fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 22:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

[PX4IO/PWM driver] Added trim values to the PWM output drivers

Browse Source
This commit is contained in:
Bartosz Wawrzacz
2016-12-10 12:56:39 +01:00
committed by Lorenz Meier
parent 4494182bfc
commit 619efa7b45
12 changed files with 222 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4fmu/fmu.cpp
+43 -1
View File
@@ -220,6 +220,7 @@ private:
uint16_t _disarmed_pwm[_max_actuators];
uint16_t _min_pwm[_max_actuators];
uint16_t _max_pwm[_max_actuators];
uint16_t _trim_pwm[_max_actuators];
uint16_t _reverse_pwm_mask;
unsigned _num_failsafe_set;
unsigned _num_disarmed_set;
@@ -342,6 +343,7 @@ PX4FMU::PX4FMU() :
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;
_max_pwm[i] = PWM_DEFAULT_MAX;
_trim_pwm[i] = PWM_DEFAULT_TRIM;
}
_control_topics[0] = ORB_ID(actuator_controls_0);
@@ -1115,7 +1117,7 @@ PX4FMU::cycle()
/* the PWM limit call takes care of out of band errors, NaN and constrains */
pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask,
_disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
_trim_pwm, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
/* overwrite outputs in case of lockdown with disarmed PWM values */
@@ -1881,6 +1883,46 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_SET_TRIM_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* allow 0 - turns the trim option off */
_trim_pwm[i] = 0;
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_trim_pwm[i] = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_trim_pwm[i] = PWM_HIGHEST_MAX;
} else {
_trim_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_TRIM_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _trim_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
case PWM_SERVO_SET(7):