forked from Archive/PX4-Autopilot
PWM limit lib: Support pre-arming
This commit is contained in:
parent
0ca6f46ef4
commit
433c9bf42d
|
@ -54,7 +54,7 @@ void pwm_limit_init(pwm_limit_t *limit)
|
|||
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 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;
|
||||
}
|
||||
|
||||
/* 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;
|
||||
|
||||
/* then set effective_pwm based on state */
|
||||
|
@ -121,6 +131,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
|
|||
|
||||
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;
|
||||
|
||||
/* if a disarmed pwm value was set, blend between disarmed and min */
|
||||
|
@ -141,8 +159,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
|
|||
ramp_min_pwm = min_pwm[i];
|
||||
}
|
||||
|
||||
float control_value = output[i];
|
||||
|
||||
if (reverse_mask & (1 << i)) {
|
||||
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];
|
||||
|
||||
/* check for invalid / disabled channels */
|
||||
if (!isfinite(control_value)) {
|
||||
effective_pwm[i] = disarmed_pwm[i];
|
||||
continue;
|
||||
}
|
||||
|
||||
if (reverse_mask & (1 << i)) {
|
||||
control_value = -1.0f * control_value;
|
||||
}
|
||||
|
|
|
@ -71,7 +71,7 @@ typedef struct {
|
|||
|
||||
__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);
|
||||
|
||||
__END_DECLS
|
||||
|
|
Loading…
Reference in New Issue