From 3b1ca99b952cc5b9dc5fb13b11bf6192a74e0f60 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 22 Mar 2016 14:24:56 +1100 Subject: [PATCH] Copter: FlightMode - convert LAND flight mode --- ArduCopter/Copter.h | 8 ++---- ArduCopter/FlightMode.h | 35 +++++++++++++++++++++++++ ArduCopter/control_land.cpp | 51 +++++++++++++++++++------------------ ArduCopter/ekf_check.cpp | 2 +- ArduCopter/flight_mode.cpp | 15 ++++------- 5 files changed, 69 insertions(+), 42 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index b064b3a2fc..86bc769b55 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -840,14 +840,8 @@ private: void flip_run(); bool guided_nogps_init(bool ignore_checks); void guided_nogps_run(); - bool land_init(bool ignore_checks); - void land_run(); - void land_gps_run(); - void land_nogps_run(); - int32_t land_get_alt_above_ground(void); void land_run_vertical_control(bool pause_descent = false); void land_run_horizontal_control(); - void land_do_not_use_GPS(); void set_mode_land_with_pause(mode_reason_t reason); bool landing_with_GPS(); bool poshold_init(bool ignore_checks); @@ -1104,6 +1098,8 @@ private: Copter::FlightMode_GUIDED flightmode_guided{*this}; + Copter::FlightMode_LAND flightmode_land{*this}; + Copter::FlightMode_LOITER flightmode_loiter{*this}; #if FRAME_CONFIG == HELI_FRAME diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index 094b08ecfa..17f8efe603 100644 --- a/ArduCopter/FlightMode.h +++ b/ArduCopter/FlightMode.h @@ -475,3 +475,38 @@ private: GuidedMode guided_mode = Guided_TakeOff; }; + + + +class FlightMode_LAND : public FlightMode { + +public: + + FlightMode_LAND(Copter &copter) : + Copter::FlightMode(copter) + { } + + bool init(bool ignore_checks) override; + void run() override; // should be called at 100hz or more + + bool requires_GPS() const override { return false; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return false; }; + bool is_autopilot() const override { return true; } + + float get_land_descent_speed(); + bool landing_with_GPS(); + void do_not_use_GPS(); + + int32_t get_alt_above_ground(void); + +protected: + + const char *name() const override { return "LAND"; } + const char *name4() const override { return "LAND"; } + +private: + + void gps_run(); + void nogps_run(); +}; diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index 5cd7cf0f65..18cfb130e5 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -1,15 +1,16 @@ #include "Copter.h" +// FIXME? why are these static? static bool land_with_gps; static uint32_t land_start_time; static bool land_pause; // land_init - initialise land controller -bool Copter::land_init(bool ignore_checks) +bool Copter::FlightMode_LAND::init(bool ignore_checks) { // check if we have GPS and decide which LAND we're going to do - land_with_gps = position_ok(); + land_with_gps = _copter.position_ok(); if (land_with_gps) { // set target to stopping point Vector3f stopping_point; @@ -39,19 +40,19 @@ bool Copter::land_init(bool ignore_checks) // land_run - runs the land controller // should be called at 100hz or more -void Copter::land_run() +void Copter::FlightMode_LAND::run() { if (land_with_gps) { - land_gps_run(); + gps_run(); }else{ - land_nogps_run(); + nogps_run(); } } // land_gps_run - runs the land controller // horizontal position controlled with loiter controller // should be called at 100hz or more -void Copter::land_gps_run() +void Copter::FlightMode_LAND::gps_run() { // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { @@ -68,7 +69,7 @@ void Copter::land_gps_run() // disarm when the landing detector says we've landed if (ap.land_complete) { - init_disarm_motors(); + _copter.init_disarm_motors(); } return; } @@ -81,24 +82,24 @@ void Copter::land_gps_run() land_pause = false; } - land_run_horizontal_control(); - land_run_vertical_control(land_pause); + _copter.land_run_horizontal_control(); + _copter.land_run_vertical_control(land_pause); } // land_nogps_run - runs the land controller // pilot controls roll and pitch angles // should be called at 100hz or more -void Copter::land_nogps_run() +void Copter::FlightMode_LAND::nogps_run() { float target_roll = 0.0f, target_pitch = 0.0f; float target_yaw_rate = 0; // process pilot inputs - if (!failsafe.radio) { - if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ + if (!_copter.failsafe.radio) { + if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && _copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high - set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); + _copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } if (g.land_repositioning) { @@ -106,7 +107,7 @@ void Copter::land_nogps_run() update_simple_mode(); // get pilot desired lean angles - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max); } // get pilot's desired yaw rate @@ -127,7 +128,7 @@ void Copter::land_nogps_run() // disarm when the landing detector says we've landed if (ap.land_complete) { - init_disarm_motors(); + _copter.init_disarm_motors(); } return; } @@ -143,21 +144,21 @@ void Copter::land_nogps_run() land_pause = false; } - land_run_vertical_control(land_pause); + _copter.land_run_vertical_control(land_pause); } /* get a height above ground estimate for landing */ -int32_t Copter::land_get_alt_above_ground(void) +int32_t Copter::FlightMode_LAND::get_alt_above_ground(void) { int32_t alt_above_ground; - if (rangefinder_alt_ok()) { - alt_above_ground = rangefinder_state.alt_cm_filt.get(); + if (_copter.rangefinder_alt_ok()) { + alt_above_ground = _copter.rangefinder_state.alt_cm_filt.get(); } else { bool navigating = pos_control->is_active_xy(); - if (!navigating || !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) { - alt_above_ground = current_loc.alt; + if (!navigating || !_copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) { + alt_above_ground = _copter.current_loc.alt; } } return alt_above_ground; @@ -176,7 +177,7 @@ void Copter::land_run_vertical_control(bool pause_descent) // compute desired velocity const float precland_acceptable_error = 15.0f; const float precland_min_descent_speed = 10.0f; - int32_t alt_above_ground = land_get_alt_above_ground(); + int32_t alt_above_ground = flightmode_land.get_alt_above_ground(); float cmb_rate = 0; if (!pause_descent) { @@ -280,7 +281,7 @@ void Copter::land_run_horizontal_control() // there is any position estimate drift after touchdown. We // limit attitude to 7 degrees below this limit and linearly // interpolate for 1m above that - int alt_above_ground = land_get_alt_above_ground(); + int alt_above_ground = flightmode_land.get_alt_above_ground(); float attitude_limit_cd = linear_interpolate(700, aparm.angle_max, alt_above_ground, g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); float total_angle_cd = norm(nav_roll, nav_pitch); @@ -299,10 +300,10 @@ void Copter::land_run_horizontal_control() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain()); } -// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch +// do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch // called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS // has no effect if we are not already in LAND mode -void Copter::land_do_not_use_GPS() +void Copter::FlightMode_LAND::do_not_use_GPS() { land_with_gps = false; } diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 0c58ad45c2..a34124f02b 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -175,7 +175,7 @@ void Copter::failsafe_ekf_event() // if flight mode is already LAND ensure it's not the GPS controlled LAND if (control_mode == LAND) { - land_do_not_use_GPS(); + flightmode_land.do_not_use_GPS(); } } diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index a16074a20f..4d17407002 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -89,7 +89,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) break; case LAND: - success = land_init(ignore_checks); + success = flightmode_land.init(ignore_checks); + if (success) { + flightmode = &flightmode_land; + } break; case RTL: @@ -206,10 +209,6 @@ void Copter::update_flight_mode() switch (control_mode) { - case LAND: - land_run(); - break; - case RTL: rtl_run(); break; @@ -377,12 +376,11 @@ void Copter::notify_flight_mode() case RTL: case AVOID_ADSB: case GUIDED_NOGPS: - case LAND: case SMART_RTL: // autopilot modes AP_Notify::flags.autopilot_mode = true; break; - default: + default: // all other are manual flight modes AP_Notify::flags.autopilot_mode = false; break; @@ -393,9 +391,6 @@ void Copter::notify_flight_mode() case RTL: notify.set_flight_mode_str("RTL "); break; - case LAND: - notify.set_flight_mode_str("LAND"); - break; case DRIFT: notify.set_flight_mode_str("DRIF"); break;