From a7521333b8efb320d03485e07c76a11aff498488 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 8 Apr 2021 16:57:13 +1000 Subject: [PATCH] Blimp: move rudder arming up to RC_Channel --- Blimp/AP_Arming.cpp | 6 ++++ Blimp/Blimp.cpp | 1 - Blimp/Blimp.h | 9 ------ Blimp/RC_Channel.cpp | 20 ++++++------- Blimp/RC_Channel.h | 12 ++++++++ Blimp/motors.cpp | 67 -------------------------------------------- 6 files changed, 28 insertions(+), 87 deletions(-) diff --git a/Blimp/AP_Arming.cpp b/Blimp/AP_Arming.cpp index 2011502b2f..45423fedd1 100644 --- a/Blimp/AP_Arming.cpp +++ b/Blimp/AP_Arming.cpp @@ -373,6 +373,12 @@ bool AP_Arming_Blimp::disarm(const AP_Arming::Method method, bool do_disarm_chec return true; } + if (method == AP_Arming::Method::RUDDER) { + if (!blimp.flightmode->has_manual_throttle() && !blimp.ap.land_complete) { + return false; + } + } + if (!AP_Arming::disarm(method, do_disarm_checks)) { return false; } diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 8ae8c0b704..12b7d8da77 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -69,7 +69,6 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_GPS, &blimp.gps, update, 50, 200, 9), SCHED_TASK(update_batt_compass, 10, 120, 12), SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&blimp.g2.rc_channels, read_aux_all, 10, 50, 15), - SCHED_TASK(arm_motors_check, 10, 50, 18), SCHED_TASK(update_altitude, 10, 100, 21), SCHED_TASK(three_hz_loop, 3, 75, 24), #if AP_SERVORELAYEVENTS_ENABLED diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index aa4d2e449d..8c3114e4c1 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.cpp b/Blimp/RC_Channel.cpp index 1132d82330..4271105541 100644 --- a/Blimp/RC_Channel.cpp +++ b/Blimp/RC_Channel.cpp @@ -107,7 +107,7 @@ bool RC_Channel_Blimp::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch if ((ch_flag == AuxSwitchPos::HIGH) && (blimp.control_mode <= Mode::Number::MANUAL) && (blimp.channel_up->get_control_in() == 0)) { - blimp.save_trim(); + blimp.g2.rc_channels.save_trim(); } break; @@ -126,26 +126,26 @@ bool RC_Channel_Blimp::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch } // save_trim - adds roll and pitch trims from the radio to ahrs -void Blimp::save_trim() +void RC_Channels_Blimp::save_trim() { // save roll and pitch trim float roll_trim = ToRad((float)channel_right->get_control_in()/100.0f); float pitch_trim = ToRad((float)channel_front->get_control_in()/100.0f); - ahrs.add_trim(roll_trim, pitch_trim); + AP::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() +void RC_Channels_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() +void RC_Channels_Blimp::auto_trim() { if (auto_trim_counter > 0) { if (blimp.flightmode != &blimp.mode_manual || @@ -158,14 +158,14 @@ void Blimp::auto_trim() AP_Notify::flags.save_trim = true; if (!auto_trim_started) { - if (ap.land_complete) { + if (blimp.ap.land_complete) { // haven't taken off yet return; } auto_trim_started = true; } - if (ap.land_complete) { + if (blimp.ap.land_complete) { // landed again. auto_trim_cancel(); return; @@ -174,14 +174,14 @@ void Blimp::auto_trim() auto_trim_counter--; // calculate roll trim adjustment - float roll_trim_adjustment = ToRad((float)channel_right->get_control_in() / 4000.0f); + const float roll_trim_adjustment = ToRad((float)blimp.channel_right->get_control_in() / 4000.0f); // calculate pitch trim adjustment - float pitch_trim_adjustment = ToRad((float)channel_front->get_control_in() / 4000.0f); + const float pitch_trim_adjustment = ToRad((float)blimp.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)); + AP::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) { diff --git a/Blimp/RC_Channel.h b/Blimp/RC_Channel.h index 4a6e5e0156..9633f2efa4 100644 --- a/Blimp/RC_Channel.h +++ b/Blimp/RC_Channel.h @@ -43,8 +43,20 @@ public: return &obj_channels[chan]; } + // auto-trim functionality; adjust ahrstrim based on momentary RC Input + void save_trim(); + void auto_trim(); + protected: int8_t flight_mode_channel_number() const override; +private: + + // Used to exit the roll and pitch auto trim function + void auto_trim_cancel(); + + uint8_t auto_trim_counter; + bool auto_trim_started = false; + }; diff --git a/Blimp/motors.cpp b/Blimp/motors.cpp index 90fb3308a9..9b2eca5ff2 100644 --- a/Blimp/motors.cpp +++ b/Blimp/motors.cpp @@ -5,73 +5,6 @@ #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 -// called at 10hz -void Blimp::arm_motors_check() -{ - static int16_t arming_counter; - - // check if arming/disarm using rudder is allowed - AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type(); - if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) { - return; - } - - // ensure throttle is down - if (channel_up->get_control_in() > 0) { //MIR what dow we do with this? - arming_counter = 0; - return; - } - - int16_t yaw_in = channel_yaw->get_control_in(); - - // full right - if (yaw_in > 900) { - - // increase the arming counter to a maximum of 1 beyond the auto trim counter - if (arming_counter <= AUTO_TRIM_DELAY) { - arming_counter++; - } - - // arm the motors and configure for flight - if (arming_counter == ARM_DELAY && !motors->armed()) { - // reset arming counter if arming fail - if (!arming.arm(AP_Arming::Method::RUDDER)) { - arming_counter = 0; - } - } - - // 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) { - arming_counter = 0; - return; - } - - // increase the counter to a maximum of 1 beyond the disarm delay - if (arming_counter <= DISARM_DELAY) { - arming_counter++; - } - - // disarm the motors - if (arming_counter == DISARM_DELAY && motors->armed()) { - arming.disarm(AP_Arming::Method::RUDDER); - } - - // Yaw is centered so reset arming counter - } else { - arming_counter = 0; - } -} - - // motors_output - send output to motors library which will adjust and send to ESCs and servos void Blimp::motors_output() {