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

add trim parameter handling

Browse Source
This commit is contained in:
Mark Whitehorn
2016-12-10 12:56:39 +01:00
committed by Lorenz Meier
parent 9ced4afca3
commit ea83b67bd8
1 changed files with 26 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4fmu/fmu.cpp
+26 -15
View File
@@ -150,6 +150,8 @@ public:
hrt_abstime edge_time, uint32_t edge_state,
uint32_t overflow);
void update_pwm_trims();
private:
enum RC_SCAN {
RC_SCAN_PPM = 0,
@@ -756,6 +758,28 @@ PX4FMU::update_pwm_rev_mask()
}
}
void
PX4FMU::update_pwm_trims()
{
struct pwm_output_values pwm;
for (unsigned i = 0; i < _max_actuators; i++) {
char pname[16];
int32_t ival;
/* fill the struct from parameters */
sprintf(pname, "PWM_AUX_TRIM%d", i + 1);
param_t param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
param_get(param_h, &ival);
pwm.values[i] = ival;
}
}
/* copy the trim values to the mixer offsets */
_mixers->set_trims(pwm.values, _max_actuators);
}
void
PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues)
{
@@ -1892,21 +1916,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
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];
}
}
/* copy the trim values to the mixer offsets */
_mixers->set_trims(pwm->values, pwm->channel_count);
break;
}