diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 596c81e547..fb3499e532 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -239,10 +239,6 @@ private: // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. uint32_t arm_time_ms; - // Used to exit the roll and pitch auto trim function - uint8_t auto_trim_counter; - bool auto_trim_started = false; - // last valid RC input time uint32_t last_radio_update_ms; @@ -412,11 +408,6 @@ private: bool rangefinder_alt_ok(); bool rangefinder_up_ok(); - // RC_Channel.cpp - void save_trim(); - void auto_trim(); - void auto_trim_cancel(); - // system.cpp void init_ardupilot() override; void startup_INS_ground(); diff --git a/Blimp/RC_Channel_Blimp.cpp b/Blimp/RC_Channel_Blimp.cpp index 5db884f88a..20e68927d3 100644 --- a/Blimp/RC_Channel_Blimp.cpp +++ b/Blimp/RC_Channel_Blimp.cpp @@ -106,14 +106,6 @@ bool RC_Channel_Blimp::do_aux_function(const AuxFuncTrigger &trigger) switch (ch_option) { - case AUX_FUNC::SAVE_TRIM: - if ((ch_flag == AuxSwitchPos::HIGH) && - (blimp.control_mode <= Mode::Number::MANUAL) && - (blimp.channel_up->get_control_in() == 0)) { - blimp.save_trim(); - } - break; - case AUX_FUNC::LOITER: do_aux_function_change_mode(Mode::Number::LOITER, ch_flag); break; @@ -127,69 +119,3 @@ bool RC_Channel_Blimp::do_aux_function(const AuxFuncTrigger &trigger) } return true; } - -// save_trim - adds roll and pitch trims from the radio to ahrs -void Blimp::save_trim() -{ - // save roll and pitch trim - float roll_trim = ToRad((float)channel_right->get_control_in()*0.01f); - float pitch_trim = ToRad((float)channel_front->get_control_in()*0.01f); - ahrs.add_trim(roll_trim, pitch_trim); - LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM); - gcs().send_text(MAV_SEVERITY_INFO, "Trim saved"); -} - -// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions -// meant to be called continuously while the pilot attempts to keep the blimp level -void Blimp::auto_trim_cancel() -{ - auto_trim_counter = 0; - AP_Notify::flags.save_trim = false; - gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim cancelled"); -} - -void Blimp::auto_trim() -{ - if (auto_trim_counter > 0) { - if (blimp.flightmode != &blimp.mode_manual || - !blimp.motors->armed()) { - auto_trim_cancel(); - return; - } - - // flash the leds - AP_Notify::flags.save_trim = true; - - if (!auto_trim_started) { - if (ap.land_complete) { - // haven't taken off yet - return; - } - auto_trim_started = true; - } - - if (ap.land_complete) { - // landed again. - auto_trim_cancel(); - return; - } - - auto_trim_counter--; - - // calculate roll trim adjustment - float roll_trim_adjustment = ToRad((float)channel_right->get_control_in() / 4000.0f); - - // calculate pitch trim adjustment - float pitch_trim_adjustment = ToRad((float)channel_front->get_control_in() / 4000.0f); - - // add trim to ahrs object - // save to eeprom on last iteration - ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0)); - - // on last iteration restore leds and accel gains to normal - if (auto_trim_counter == 0) { - AP_Notify::flags.save_trim = false; - gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim: Trims saved"); - } - } -} diff --git a/Blimp/motors.cpp b/Blimp/motors.cpp index 90fb3308a9..d2e6cfe7d2 100644 --- a/Blimp/motors.cpp +++ b/Blimp/motors.cpp @@ -2,7 +2,6 @@ #define ARM_DELAY 20 // called at 10hz so 2 seconds #define DISARM_DELAY 20 // called at 10hz so 2 seconds -#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds #define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second // arm_motors_check - checks for pilot input to arm or disarm the blimp @@ -29,7 +28,7 @@ void Blimp::arm_motors_check() if (yaw_in > 900) { // increase the arming counter to a maximum of 1 beyond the auto trim counter - if (arming_counter <= AUTO_TRIM_DELAY) { + if (arming_counter < ARM_DELAY) { arming_counter++; } @@ -41,13 +40,6 @@ void Blimp::arm_motors_check() } } - // arm the motors and configure for flight - if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && control_mode == Mode::Number::MANUAL) { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start"); - auto_trim_counter = 250; - auto_trim_started = false; - } - // full left and rudder disarming is enabled } else if ((yaw_in < -900) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) { if (!flightmode->has_manual_throttle() && !ap.land_complete) {