Skip to content
Snippets Groups Projects
Commit b374f604 authored by Andrew Tridgell's avatar Andrew Tridgell
Browse files

APM: simplify radio_trim code

this removes the duplicate code. Throttle trim is not changed.
parent a90182b9
No related branches found
No related tags found
No related merge requests found
...@@ -183,35 +183,14 @@ static void trim_control_surfaces() ...@@ -183,35 +183,14 @@ static void trim_control_surfaces()
// save to eeprom // save to eeprom
g.channel_roll.save_eeprom(); g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom(); g.channel_pitch.save_eeprom();
g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom(); g.channel_rudder.save_eeprom();
} }
static void trim_radio() static void trim_radio()
{ {
for (int y = 0; y < 30; y++) { for (uint8_t y = 0; y < 30; y++) {
read_radio(); read_radio();
} }
// Store the trim values trim_control_surfaces();
// ---------------------
if(g.mix_mode == 0) {
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_aileron);
} else {
elevon1_trim = ch1_temp;
elevon2_trim = ch2_temp;
uint16_t center = 1500;
g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center;
}
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
// save to eeprom
g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom();
//g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment