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

pwm handler: Support channel reversal

This commit is contained in:
Lorenz Meier 2015-05-23 13:15:48 +02:00
parent c6fe4fd35a
commit 3883b71132
2 changed files with 19 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -53,7 +53,9 @@ void pwm_limit_init(pwm_limit_t *limit)
return;
}
void pwm_limit_calc(const bool armed, const unsigned num_channels, 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)
void pwm_limit_calc(const bool 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)
{
/* first evaluate state changes */
@ -134,7 +136,13 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
ramp_min_pwm = min_pwm[i];
}
effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
float control_value = output[i];
if (reverse_mask & (1 << i)) {
control_value = -1.0f * control_value;
}
effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
/* last line of defense against invalid inputs */
if (effective_pwm[i] < ramp_min_pwm) {
@ -147,7 +155,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
break;
case PWM_LIMIT_STATE_ON:
for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
float control_value = output[i];
if (reverse_mask & (1 << i)) {
control_value = -1.0f * control_value;
}
effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
/* last line of defense against invalid inputs */
if (effective_pwm[i] < min_pwm[i]) {

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

@ -72,7 +72,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 *disarmed_pwm,
__EXPORT void pwm_limit_calc(const bool 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