dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FMU PWM parameters respect instance for MAIN/AUX usage

This commit is contained in:
Daniel Agar 2018-07-16 10:32:26 -04:00
parent ab044e274d
commit ab2d595224
1 changed files with 30 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
src/drivers/px4fmu/fmu.cpp
View File

@ -515,8 +515,6 @@ PX4FMU::init()
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
update_pwm_rev_mask();
#ifdef RC_SERIAL_PORT
# ifdef RF_RADIO_POWER_CONTROL
@ -537,12 +535,6 @@ PX4FMU::init()
// Getting initial parameter values
update_params();
for (unsigned i = 0; i < _max_actuators; i++) {
char pname[16];
sprintf(pname, "PWM_AUX_TRIM%d", i + 1);
param_find(pname);
}
return 0;
}
@ -926,15 +918,28 @@ PX4FMU::update_pwm_rev_mask()
{
_reverse_pwm_mask = 0;
const char *pname_format;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
pname_format = "PWM_MAIN_REV%d";
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
pname_format = "PWM_AUX_REV%d";
} else {
PX4_ERR("PWM REV only for MAIN and AUX");
return;
}
for (unsigned i = 0; i < _max_actuators; i++) {
char pname[16];
int32_t ival;
/* fill the channel reverse mask from parameters */
sprintf(pname, "PWM_AUX_REV%d", i + 1);
sprintf(pname, pname_format, i + 1);
param_t param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
int32_t ival = 0;
param_get(param_h, &ival);
_reverse_pwm_mask |= ((int16_t)(ival != 0)) << i;
}
@ -950,15 +955,28 @@ PX4FMU::update_pwm_trims()
int16_t values[_max_actuators] = {};
const char *pname_format;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
pname_format = "PWM_MAIN_TRIM%d";
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
pname_format = "PWM_AUX_TRIM%d";
} else {
PX4_ERR("PWM TRIM only for MAIN and AUX");
return;
}
for (unsigned i = 0; i < _max_actuators; i++) {
char pname[16];
float pval;
/* fill the struct from parameters */
sprintf(pname, "PWM_AUX_TRIM%d", i + 1);
sprintf(pname, pname_format, i + 1);
param_t param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
float pval = 0.0f;
param_get(param_h, &pval);
values[i] = (int16_t)(10000 * pval);
PX4_DEBUG("%s: %d", pname, values[i]);