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

change trim units from absolute usec to normalized values

This commit is contained in:
Mark Whitehorn 2016-11-09 13:45:36 -07:00 committed by Lorenz Meier
parent d8528f46ce
commit dbc149c224
11 changed files with 48 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/px4params/xmlout.py
View File

@ -25,7 +25,7 @@ class XMLOutput():
xml_version = ET.SubElement(xml_parameters, "parameter_version_major")
xml_version.text = "1"
xml_version = ET.SubElement(xml_parameters, "parameter_version_minor")
xml_version.text = "11"
xml_version.text = "12"
importtree = ET.parse(inject_xml_file_name)
injectgroups = importtree.getroot().findall("group")
for igroup in injectgroups:

2
src/drivers/drv_pwm_output.h
View File

@ -114,7 +114,7 @@ __BEGIN_DECLS
/**
* Default trim PWM in us
*/
#define PWM_DEFAULT_TRIM 1500
#define PWM_DEFAULT_TRIM 0
/**
* Lowest PWM allowed as the maximum PWM

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

@ -775,19 +775,19 @@ PX4FMU::update_pwm_trims()
} else {
uint16_t values[_max_actuators] = {};
int16_t values[_max_actuators] = {};
for (unsigned i = 0; i < _max_actuators; i++) {
char pname[16];
int32_t ival;
float pval;
/* 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);
values[i] = ival;
param_get(param_h, &pval);
values[i] = (int16_t)(10000 * pval);
PX4_DEBUG("%s: %d", pname, values[i]);
}
}
@ -1176,9 +1176,8 @@ PX4FMU::cycle()
uint16_t pwm_limited[_max_actuators];
/* the PWM limit call takes care of out of band errors, NaN and constrains */
// TODO: remove trim_pwm parameter
pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask,
_disarmed_pwm, _min_pwm, _max_pwm, _trim_pwm, outputs, pwm_limited, &_pwm_limit);
_disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
/* overwrite outputs in case of lockdown with disarmed PWM values */
@ -1963,7 +1962,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
}
/* copy the trim values to the mixer offsets */
_mixers->set_trims(pwm->values, pwm->channel_count);
_mixers->set_trims((int16_t *)pwm->values, pwm->channel_count);
PX4_DEBUG("set_trims: %d, %d, %d, %d", pwm->values[0], pwm->values[1], pwm->values[2], pwm->values[3]);
break;

42
src/drivers/px4fmu/px4fmu_params.c
View File

@ -113,64 +113,70 @@ PARAM_DEFINE_INT32(PWM_AUX_REV6, 0);
*
* Set to neutral period in usec
*
* @min 1400
* @max 1600
* @min -0.2
* @max 0.2
* @decimal 2
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_AUX_TRIM1, 1500);
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM1, 0);
/**
* Trim value for FMU PWM output channel 2
*
* Set to neutral period in usec
*
* @min 1400
* @max 1600
* @min -0.2
* @max 0.2
* @decimal 2
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_AUX_TRIM2, 1500);
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM2, 0);
/**
* Trim value for FMU PWM output channel 3
*
* Set to neutral period in usec
*
* @min 1400
* @max 1600
* @min -0.2
* @max 0.2
* @decimal 2
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_AUX_TRIM3, 1500);
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM3, 0);
/**
* Trim value for FMU PWM output channel 4
*
* Set to neutral period in usec
*
* @min 1400
* @max 1600
* @min -0.2
* @max 0.2
* @decimal 2
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_AUX_TRIM4, 1500);
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM4, 0);
/**
* Trim value for FMU PWM output channel 5
*
* Set to neutral period in usec
*
* @min 1400
* @max 1600
* @min -0.2
* @max 0.2
* @decimal 2
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_AUX_TRIM5, 1500);
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM5, 0);
/**
* Trim value for FMU PWM output channel 6
*
* Set to neutral period in usec
*
* @min 1400
* @max 1600
* @min -0.2
* @max 0.2
* @decimal 2
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_AUX_TRIM6, 1500);
PARAM_DEFINE_FLOAT(PWM_AUX_TRIM6, 0);

2
src/modules/px4iofirmware/mixer.cpp
View File

@ -256,7 +256,7 @@ mixer_tick(void)
/* the pwm limit call takes care of out of band errors */
pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
r_page_servo_control_min, r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit);
r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
/* clamp unused outputs to zero */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {

2
src/modules/px4iofirmware/px4io.h
View File

@ -90,7 +90,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
extern uint16_t r_page_servo_control_trim[]; /* PX4IO_PAGE_CONTROL_TRIM_PWM */
extern int16_t r_page_servo_control_trim[]; /* PX4IO_PAGE_CONTROL_TRIM_PWM */
extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
/*

4
src/modules/px4iofirmware/registers.c
View File

@ -260,10 +260,10 @@ uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_D
/**
* PAGE 108
*
* trim PWM values for center position
* trim values for center position
*
*/
uint16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM };
int16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM };
/**
* PAGE 109

2
src/modules/systemlib/mixer/mixer.h
View File

@ -338,7 +338,7 @@ public:
* Invoke the set_offset method of each mixer in the group
* for each value in page r_page_servo_control_trim
*/
unsigned set_trims(uint16_t *v, unsigned n);
unsigned set_trims(int16_t *v, unsigned n);
unsigned set_trim(float trim)
{

18
src/modules/systemlib/mixer/mixer_group.cpp
View File

@ -112,21 +112,21 @@ MixerGroup::mix(float *outputs, unsigned space, uint16_t *status_reg)
return index;
}
/*
* set_trims() has no effect except for the SimpleMixer implementation for which set_trim()
* always returns the value one.
* The only other existing implementation is MultirotorMixer, which ignores the trim value
* and returns _rotor_count.
*/
unsigned
MixerGroup::set_trims(uint16_t *values, unsigned n)
MixerGroup::set_trims(int16_t *values, unsigned n)
{
Mixer *mixer = _first;
unsigned index = 0;
while ((mixer != nullptr) && (index < n)) {
/*
* hardwired assumption that PWM output range is [1000, 2000] usec
*
* This only works with SimpleMixer::set_trim(float) which always returns the value one,
* but the only other existing implementation is MultirotorMixer, which ignores
* the trim value.
*/
float offset = ((float)values[index] - 1500) / 500;
/* convert from integer to float */
float offset = (float)values[index] / 10000;
/* to be safe, clamp offset to range of [-100, 100] usec */
if (offset < -0.2f) { offset = -0.2f; }

10
src/modules/systemlib/pwm_limit/pwm_limit.c
View File

@ -56,7 +56,7 @@ void pwm_limit_init(pwm_limit_t *limit)
}
void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask,
const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const uint16_t *trim_pwm,
const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm,
const float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
{
@ -207,16 +207,8 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c
control_value = -1.0f * control_value;
}
// if (trim_pwm[i] == 0) {
effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2;
// } else if (control_value < 0) {
// effective_pwm[i] = control_value * (trim_pwm[i] - min_pwm[i]) + trim_pwm[i];
//
// } else {
// effective_pwm[i] = control_value * (max_pwm[i] - trim_pwm[i]) + trim_pwm[i];
// }
/* last line of defense against invalid inputs */
if (effective_pwm[i] < min_pwm[i]) {
effective_pwm[i] = min_pwm[i];

2
src/modules/systemlib/pwm_limit/pwm_limit.h
View File

@ -73,7 +73,7 @@ __EXPORT void pwm_limit_init(pwm_limit_t *limit);
__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels,
const uint16_t reverse_mask, const uint16_t *disarmed_pwm,
const uint16_t *min_pwm, const uint16_t *max_pwm, const uint16_t *trim_pwm,
const uint16_t *min_pwm, const uint16_t *max_pwm,
const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
__END_DECLS