diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-21 20:04:21 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-21 20:04:21 +0200 |
commit | c402ac28ecb1d59e1511ee90804a96ac47d7793e (patch) | |
tree | a0264190f1416d9dde39027cf0b64b828f904cf7 /src/drivers | |
parent | 0f01905f9a6bb30ff51da365ff5e05aa70a9c937 (diff) | |
parent | aa8fcceea219c83d40354f2eeec84f738108bc0f (diff) | |
download | px4-firmware-c402ac28ecb1d59e1511ee90804a96ac47d7793e.tar.gz px4-firmware-c402ac28ecb1d59e1511ee90804a96ac47d7793e.tar.bz2 px4-firmware-c402ac28ecb1d59e1511ee90804a96ac47d7793e.zip |
Merge remote-tracking branch 'upstream/master' into obcfailsafe
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/pca9685/pca9685.cpp | 26 |
1 files changed, 15 insertions, 11 deletions
diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index c97d1e9ab..6f69ce8a1 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -98,13 +98,15 @@ #define PCA9685_PWMFREQ 60.0f #define PCA9685_NCHANS 16 // total amount of pwm outputs -#define PCA9685_SERVOMIN 150 // this is the 'minimum' pulse length count (out of 4096) -#define PCA9685_SERVOMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f +#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f -#define PCA9685_HALFRANGE ((PCA9685_SERVOMAX - PCA9685_SERVOMIN)/2) -#define PCA9685_CENTER (PCA9685_SERVOMIN + PCA9685_HALFRANGE) -#define PCA9685_MAXSERVODEG 45 //maximal defelction in degrees -#define PCA9685_SCALE (PCA9685_HALFRANGE / (M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) +#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2) +#define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees + PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG + PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG + */ +#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -300,7 +302,7 @@ void PCA9685::i2cpwm() { if (_mode == IOX_MODE_TEST_OUT) { - setPin(0, PCA9685_CENTER); + setPin(0, PCA9685_PWMCENTER); _should_run = true; } else if (_mode == IOX_MODE_OFF) { _should_run = false; @@ -320,14 +322,16 @@ PCA9685::i2cpwm() if (updated) { orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - uint16_t new_value = PCA9685_CENTER + - (_actuator_controls.control[i] * PCA9685_SCALE); + /* Scale the controls to PWM, first multiply by pi to get rad, + * the control[i] values are on the range -1 ... 1 */ + uint16_t new_value = PCA9685_PWMCENTER + + (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE); debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, (double)_actuator_controls.control[i]); if (new_value != _current_values[i] && isfinite(new_value) && - new_value >= 0 && - new_value <= PCA9685_SERVOMAX) { + new_value >= PCA9685_PWMMIN && + new_value <= PCA9685_PWMMAX) { /* This value was updated, send the command to adjust the PWM value */ setPin(i, new_value); _current_values[i] = new_value; |