aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

RC_Channel: support automatic servo trimming

Browse Source
This commit is contained in:
Andrew Tridgell 2016-10-15 21:41:04 +11:00
parent e391da7aec
commit f9dd31b6e5
2 changed files with 58 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/RC_Channel/SRV_Channel.cpp
View File

@ -168,6 +168,13 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = {
// @Values: -1:Reversed,1:Normal
// @User: Advanced
AP_GROUPINFO("4_REV", 17, SRV_Channels, reverse[3], 1),
// @Param: _AUTO_TRIM
// @DisplayName: Automatic servo trim
// @Description: This enables automatic servo trim in flight. Servos will be trimed in stabilized flight modes when the aircraft is close to level. Changes to servo trim will be saved every 10 seconds and will persist between flights.
// @Values: 0:Disable,1:Enable
// @User: Advanced
AP_GROUPINFO("_AUTO_TRIM", 18, SRV_Channels, auto_trim, 0),
AP_GROUPEND
};
@ -269,3 +276,39 @@ void SRV_Channels::set_esc_scaling(uint8_t chnum)
hal.rcout->set_esc_scaling(servo_min[chnum], servo_max[chnum]);
}
}
/*
auto-adjust channel trim from an integrator value. Positive v means
adjust trim up. Negative means decrease
*/
void SRV_Channels::adjust_trim(uint8_t chnum, float v)
{
if (reverse[chnum] == -1) {
v = -v;
}
uint16_t new_trim = servo_trim[chnum];
float trim_scaled = float(servo_trim[chnum] - servo_min[chnum]) / (servo_max[chnum] - servo_min[chnum]);
if (v > 0 && trim_scaled < 0.6f) {
new_trim++;
} else if (v < 0 && trim_scaled > 0.4f) {
new_trim--;
} else {
return;
}
servo_trim[chnum].set(new_trim);
trimmed_mask |= 1U<<chnum;
}
/*
save adjusted trims
*/
void SRV_Channels::save_trim(void)
{
for (uint8_t i=0; i<NUM_SERVO_RANGE_CHANNELS; i++) {
if (trimmed_mask & (1U<<i)) {
servo_trim[i].set_and_save(servo_trim[i].get());
}
}
trimmed_mask = 0;
}

15
libraries/RC_Channel/SRV_Channel.h
View File

@ -39,12 +39,25 @@ public:
// setup output ESC scaling for a channel
void set_esc_scaling(uint8_t chnum);
// return true when enabled
bool enabled(void) const { return enable; }
// return true when auto_trim enabled
bool auto_trim_enabled(void) const { return auto_trim; }
// adjust trim of a channel by a small increment
void adjust_trim(uint8_t ch, float v);
// save trims
void save_trim(void);
private:
AP_Int8 enable;
int8_t esc_cal_chan;
bool last_enable;
uint8_t trimmed_mask;
// PWM values for min/max and trim
AP_Int16 servo_min[NUM_SERVO_RANGE_CHANNELS];
@ -54,6 +67,8 @@ private:
// reversal, following convention that < 0 means reversed, >= 0 means normal
AP_Int8 reverse[NUM_SERVO_RANGE_CHANNELS];
AP_Int8 auto_trim;
// remap a PWM value from a channel in value
uint16_t remap_pwm(uint8_t ch, uint16_t pwm) const;
};