From cafce01357ef01f5c5881f4ef86cf7629b6f0c29 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Mar 2016 11:51:39 +1100 Subject: [PATCH] Copter: FlightMode - convert POSHOLD flight mode --- ArduCopter/Copter.h | 13 ++++-------- ArduCopter/FlightMode.h | 37 ++++++++++++++++++++++++++++++++++ ArduCopter/control_poshold.cpp | 31 ++++++++++++++-------------- ArduCopter/flight_mode.cpp | 15 ++++---------- 4 files changed, 61 insertions(+), 35 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 649dcc9a17..95c03de1f2 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -788,15 +788,6 @@ private: void land_run_horizontal_control(); void set_mode_land_with_pause(mode_reason_t reason); bool landing_with_GPS(); - bool poshold_init(bool ignore_checks); - void poshold_run(); - void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); - int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control); - void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity); - void poshold_update_wind_comp_estimate(); - void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle); - void poshold_roll_controller_to_pilot_override(); - void poshold_pitch_controller_to_pilot_override(); // Throw to launch functionality bool throw_init(bool ignore_checks); @@ -1039,6 +1030,10 @@ private: Copter::FlightMode_LOITER flightmode_loiter{*this}; +#if POSHOLD_ENABLED == ENABLED + Copter::FlightMode_POSHOLD flightmode_poshold{*this}; +#endif + Copter::FlightMode_RTL flightmode_rtl{*this}; #if FRAME_CONFIG == HELI_FRAME diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index d8b67b5f4e..5776baf7b7 100644 --- a/ArduCopter/FlightMode.h +++ b/ArduCopter/FlightMode.h @@ -729,3 +729,40 @@ private: bool autotune_currently_level(); }; #endif + + + +#if POSHOLD_ENABLED == ENABLED +class FlightMode_POSHOLD : public FlightMode { + +public: + + FlightMode_POSHOLD(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 true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; }; + bool is_autopilot() const override { return false; } + +protected: + + const char *name() const override { return "POSHOLD"; } + const char *name4() const override { return "PHLD"; } + +private: + + void poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); + int16_t poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control); + void poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity); + void poshold_update_wind_comp_estimate(); + void poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle); + void poshold_roll_controller_to_pilot_override(); + void poshold_pitch_controller_to_pilot_override(); + +}; +#endif diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index 67025eeb2c..46c4de04b8 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -73,10 +73,10 @@ static struct { } poshold; // poshold_init - initialise PosHold controller -bool Copter::poshold_init(bool ignore_checks) +bool Copter::FlightMode_POSHOLD::init(bool ignore_checks) { // fail to initialise PosHold mode if no GPS lock - if (!position_ok() && !ignore_checks) { + if (!_copter.position_ok() && !ignore_checks) { return false; } @@ -124,7 +124,7 @@ bool Copter::poshold_init(bool ignore_checks) // poshold_run - runs the PosHold controller // should be called at 100hz or more -void Copter::poshold_run() +void Copter::FlightMode_POSHOLD::run() { float target_roll, target_pitch; // pilot's roll and pitch angle inputs float target_yaw_rate = 0; // pilot desired yaw rate in centi-degrees/sec @@ -150,7 +150,7 @@ void Copter::poshold_run() } // process pilot inputs - if (!failsafe.radio) { + if (!_copter.failsafe.radio) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); @@ -204,7 +204,7 @@ void Copter::poshold_run() return; }else{ // convert pilot input to 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); // convert inertial nav earth-frame velocities to body-frame // To-Do: move this to AP_Math (or perhaps we already have a function to do this) @@ -520,14 +520,15 @@ void Copter::poshold_run() } // constrain target pitch/roll angles - poshold.roll = constrain_int16(poshold.roll, -aparm.angle_max, aparm.angle_max); - poshold.pitch = constrain_int16(poshold.pitch, -aparm.angle_max, aparm.angle_max); + float angle_max = _copter.aparm.angle_max; + poshold.roll = constrain_int16(poshold.roll, -angle_max, angle_max); + poshold.pitch = constrain_int16(poshold.pitch, -angle_max, angle_max); // update attitude controller targets attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate, get_smoothing_gain()); // adjust climb rate using rangefinder - if (rangefinder_alt_ok()) { + if (_copter.rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); } @@ -543,7 +544,7 @@ void Copter::poshold_run() } // poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received -void Copter::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) +void Copter::FlightMode_POSHOLD::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw) { // if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) { @@ -565,7 +566,7 @@ void Copter::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float & // poshold_mix_controls - mixes two controls based on the mix_ratio // mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly -int16_t Copter::poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) +int16_t Copter::FlightMode_POSHOLD::poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control) { mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f); return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control)); @@ -574,7 +575,7 @@ int16_t Copter::poshold_mix_controls(float mix_ratio, int16_t first_control, int // poshold_update_brake_angle_from_velocity - updates the brake_angle based on the vehicle's velocity and brake_gain // brake_angle is slewed with the wpnav.poshold_brake_rate and constrained by the wpnav.poshold_braking_angle_max // velocity is assumed to be in the same direction as lean angle so for pitch you should provide the velocity backwards (i.e. -ve forward velocity) -void Copter::poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) +void Copter::FlightMode_POSHOLD::poshold_update_brake_angle_from_velocity(int16_t &brake_angle, float velocity) { float lean_angle; int16_t brake_rate = g.poshold_brake_rate; @@ -600,7 +601,7 @@ void Copter::poshold_update_brake_angle_from_velocity(int16_t &brake_angle, floa // poshold_update_wind_comp_estimate - updates wind compensation estimate // should be called at the maximum loop rate when loiter is engaged -void Copter::poshold_update_wind_comp_estimate() +void Copter::FlightMode_POSHOLD::poshold_update_wind_comp_estimate() { // check wind estimate start has not been delayed if (poshold.wind_comp_start_timer > 0) { @@ -636,7 +637,7 @@ void Copter::poshold_update_wind_comp_estimate() // poshold_get_wind_comp_lean_angles - retrieve wind compensation angles in body frame roll and pitch angles // should be called at the maximum loop rate -void Copter::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) +void Copter::FlightMode_POSHOLD::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pitch_angle) { // reduce rate to 10hz poshold.wind_comp_timer++; @@ -651,7 +652,7 @@ void Copter::poshold_get_wind_comp_lean_angles(int16_t &roll_angle, int16_t &pit } // poshold_roll_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::poshold_roll_controller_to_pilot_override() +void Copter::FlightMode_POSHOLD::poshold_roll_controller_to_pilot_override() { poshold.roll_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; poshold.controller_to_pilot_timer_roll = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; @@ -662,7 +663,7 @@ void Copter::poshold_roll_controller_to_pilot_override() } // poshold_pitch_controller_to_pilot_override - initialises transition from a controller submode (brake or loiter) to a pilot override on roll axis -void Copter::poshold_pitch_controller_to_pilot_override() +void Copter::FlightMode_POSHOLD::poshold_pitch_controller_to_pilot_override() { poshold.pitch_mode = POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE; poshold.controller_to_pilot_timer_pitch = POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 095d57fc68..8b6c6a9d28 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -134,7 +134,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) #if POSHOLD_ENABLED == ENABLED case POSHOLD: - success = poshold_init(ignore_checks); + success = flightmode_poshold.init(ignore_checks); + if (success) { + flightmode = &flightmode_poshold; + } break; #endif @@ -224,12 +227,6 @@ void Copter::update_flight_mode() switch (control_mode) { -#if POSHOLD_ENABLED == ENABLED - case POSHOLD: - poshold_run(); - break; -#endif - case BRAKE: brake_run(); break; @@ -317,7 +314,6 @@ bool Copter::mode_requires_GPS() } switch (control_mode) { case SMART_RTL: - case POSHOLD: case BRAKE: case AVOID_ADSB: case THROW: @@ -378,9 +374,6 @@ void Copter::notify_flight_mode() // set flight mode string switch (control_mode) { - case POSHOLD: - notify.set_flight_mode_str("PHLD"); - break; case BRAKE: notify.set_flight_mode_str("BRAK"); break;