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

PWM limit lib: Support pre-arming

This commit is contained in:
Lorenz Meier 2015-07-04 09:52:15 +02:00
parent 0ca6f46ef4
commit 433c9bf42d
2 changed files with 26 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -54,7 +54,7 @@ void pwm_limit_init(pwm_limit_t *limit)
return; return;
} }
void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask, 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 *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm,
const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) const float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
{ {
@ -99,6 +99,16 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
break; break;
} }
/* if the system is pre-armed, the limit state is temporarily on,
* as some outputs are valid and the non-valid outputs have been
* set to NaN. This is not stored in the state machine though,
* as the throttle channels need to go through the ramp at
* regular arming time.
*/
if (pre_armed) {
limit->state = PWM_LIMIT_STATE_ON;
}
unsigned progress; unsigned progress;
/* then set effective_pwm based on state */ /* then set effective_pwm based on state */
@ -120,6 +130,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
} }
for (unsigned i=0; i<num_channels; i++) { for (unsigned i=0; i<num_channels; i++) {
float control_value = output[i];
/* check for invalid / disabled channels */
if (!isfinite(control_value)) {
effective_pwm[i] = disarmed_pwm[i];
continue;
}
uint16_t ramp_min_pwm; uint16_t ramp_min_pwm;
@ -141,8 +159,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
ramp_min_pwm = min_pwm[i]; ramp_min_pwm = min_pwm[i];
} }
float control_value = output[i];
if (reverse_mask & (1 << i)) { if (reverse_mask & (1 << i)) {
control_value = -1.0f * control_value; control_value = -1.0f * control_value;
} }
@ -163,6 +179,12 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
float control_value = output[i]; float control_value = output[i];
/* check for invalid / disabled channels */
if (!isfinite(control_value)) {
effective_pwm[i] = disarmed_pwm[i];
continue;
}
if (reverse_mask & (1 << i)) { if (reverse_mask & (1 << i)) {
control_value = -1.0f * control_value; control_value = -1.0f * control_value;
} }

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

@ -71,7 +71,7 @@ typedef struct {
__EXPORT void pwm_limit_init(pwm_limit_t *limit); __EXPORT void pwm_limit_init(pwm_limit_t *limit);
__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm, __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 float *output, uint16_t *effective_pwm, pwm_limit_t *limit); const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
__END_DECLS __END_DECLS