From 99a22a263dc45400c7d88cd501270c6d208f6fee Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 21 Mar 2016 15:33:42 +1100 Subject: [PATCH] Copter: a FlightMode base class --- ArduCopter/Copter.h | 11 ++- ArduCopter/FlightMode.h | 153 +++++++++++++++++++++++++++++++++++++ ArduCopter/flight_mode.cpp | 23 +++++- 3 files changed, 185 insertions(+), 2 deletions(-) create mode 100644 ArduCopter/FlightMode.h diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5f7bc40ae9..d8ee47f447 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -522,7 +522,12 @@ private: // Attitude, Position and Waypoint navigation objects // To-Do: move inertial nav up or other navigation variables down here - AC_AttitudeControl *attitude_control; +#if FRAME_CONFIG == HELI_FRAME + #define AC_AttitudeControl_t AC_AttitudeControl_Heli +#else + #define AC_AttitudeControl_t AC_AttitudeControl_Multi +#endif + AC_AttitudeControl_t *attitude_control; AC_PosControl *pos_control; AC_WPNav *wp_nav; AC_Circle *circle_nav; @@ -1160,6 +1165,10 @@ private: uint16_t get_pilot_speed_dn(); +#include "FlightMode.h" + + Copter::FlightMode *flightmode; + public: void mavlink_delay_cb(); void failsafe_check(); diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h new file mode 100644 index 0000000000..134dc2adf3 --- /dev/null +++ b/ArduCopter/FlightMode.h @@ -0,0 +1,153 @@ +#pragma once + +// this class is #included into the Copter:: namespace + +class FlightMode { + friend class Copter; + friend class AP_Arming_Copter; + +public: + + FlightMode(Copter &copter) : + _copter(copter), + g(copter.g), + g2(copter.g2), + wp_nav(_copter.wp_nav), + pos_control(_copter.pos_control), + inertial_nav(_copter.inertial_nav), + ahrs(_copter.ahrs), + attitude_control(_copter.attitude_control), + motors(_copter.motors), + channel_roll(_copter.channel_roll), + channel_pitch(_copter.channel_pitch), + channel_throttle(_copter.channel_throttle), + channel_yaw(_copter.channel_yaw), + G_Dt(_copter.G_Dt), + ap(_copter.ap), + takeoff_state(_copter.takeoff_state), + ekfGndSpdLimit(_copter.ekfGndSpdLimit), + ekfNavVelGainScaler(_copter.ekfNavVelGainScaler), + auto_yaw_mode(_copter.auto_yaw_mode) + { }; + +protected: + + virtual bool init(bool ignore_checks) = 0; + virtual void run() = 0; // should be called at 100hz or more + + virtual bool is_autopilot() const { return false; } + virtual bool requires_GPS() const = 0; + virtual bool has_manual_throttle() const = 0; + virtual bool allows_arming(bool from_gcs) const = 0; + void print_FlightMode(AP_HAL::BetterStream *port) const { + port->print(name()); + } + virtual const char *name() const = 0; + + // returns a string for this flightmode, exactly 4 bytes + virtual const char *name4() const = 0; + + Copter &_copter; + + // convenience references to avoid code churn in conversion: + Parameters &g; + ParametersG2 &g2; + AC_WPNav *&wp_nav; + AC_PosControl *&pos_control; + AP_InertialNav &inertial_nav; + AP_AHRS &ahrs; + AC_AttitudeControl_t *&attitude_control; + MOTOR_CLASS *&motors; + RC_Channel *&channel_roll; + RC_Channel *&channel_pitch; + RC_Channel *&channel_throttle; + RC_Channel *&channel_yaw; + float &G_Dt; + ap_t ≈ + takeoff_state_t &takeoff_state; + + // gnd speed limit required to observe optical flow sensor limits + float &ekfGndSpdLimit; + + // scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise + float &ekfNavVelGainScaler; + + // Navigation Yaw control + // auto flight mode's yaw mode + uint8_t &auto_yaw_mode; + + // pass-through functions to reduce code churn on conversion; + // these are candidates for moving into the FlightMode base + // class. + virtual void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max) { + _copter.get_pilot_desired_lean_angles(roll_in, pitch_in, roll_out, pitch_out, angle_max); + } + virtual float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) { + return _copter.get_surface_tracking_climb_rate(target_rate, current_alt_target, dt); + } + virtual float get_pilot_desired_yaw_rate(int16_t stick_angle) { + return _copter.get_pilot_desired_yaw_rate(stick_angle); + } + virtual float get_pilot_desired_climb_rate(float throttle_control) { + return _copter.get_pilot_desired_climb_rate(throttle_control); + } + virtual float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f) { + return _copter.get_pilot_desired_throttle(throttle_control, thr_mid); + } + virtual float get_non_takeoff_throttle() { + return _copter.get_non_takeoff_throttle(); + } + virtual void update_simple_mode(void) { + _copter.update_simple_mode(); + } + virtual float get_smoothing_gain() { + return _copter.get_smoothing_gain(); + } + virtual bool set_mode(control_mode_t mode, mode_reason_t reason) { + return _copter.set_mode(mode, reason); + } + virtual void set_land_complete(bool b) { + return _copter.set_land_complete(b); + } + GCS_Copter &gcs() { + return _copter.gcs(); + } + virtual void Log_Write_Event(uint8_t id) { + return _copter.Log_Write_Event(id); + } + virtual void set_throttle_takeoff() { + return _copter.set_throttle_takeoff(); + } + virtual void set_auto_yaw_mode(uint8_t yaw_mode) { + return _copter.set_auto_yaw_mode(yaw_mode); + } + void set_auto_yaw_rate(float turn_rate_cds) { + return _copter.set_auto_yaw_rate(turn_rate_cds); + } + void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle) { + return _copter.set_auto_yaw_look_at_heading(angle_deg, turn_rate_dps, direction, relative_angle); + } + virtual void takeoff_timer_start(float alt_cm) { + return _copter.takeoff_timer_start(alt_cm); + } + virtual void takeoff_stop() { + return _copter.takeoff_stop(); + } + virtual void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate) { + return _copter.takeoff_get_climb_rates(pilot_climb_rate, takeoff_climb_rate); + } + float get_auto_heading() { + return _copter.get_auto_heading(); + } + float get_auto_yaw_rate_cds() { + return _copter.get_auto_yaw_rate_cds(); + } + float get_avoidance_adjusted_climbrate(float target_rate) { + return _copter.get_avoidance_adjusted_climbrate(target_rate); + } + uint16_t get_pilot_speed_dn() { + return _copter.get_pilot_speed_dn(); + } + + // end pass-through functions +}; diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 66b5c54a8a..d1c24df2b6 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -24,7 +24,6 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) return true; } - #if FRAME_CONFIG == HELI_FRAME // do not allow helis to enter a non-manual throttle mode if the // rotor runup is not complete @@ -33,6 +32,12 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) } #endif + // for transition, we assume no flightmode object will be used in + // the new mode, and if the transition fails we reset the + // flightmode to the previous value + Copter::FlightMode* old_flightmode = flightmode; + flightmode = nullptr; + switch (mode) { case ACRO: #if FRAME_CONFIG == HELI_FRAME @@ -160,6 +165,7 @@ failed: #endif } else { + flightmode = old_flightmode; // Log error that we failed to enter desired flight mode Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode); gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed"); @@ -181,6 +187,10 @@ void Copter::update_flight_mode() // Update EKF speed limit - used to limit speed when we are using optical flow ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); + if (flightmode != nullptr) { + flightmode->run(); + } + switch (control_mode) { case ACRO: #if FRAME_CONFIG == HELI_FRAME @@ -332,6 +342,9 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr // returns true or false whether mode requires GPS bool Copter::mode_requires_GPS(control_mode_t mode) { + if (flightmode != nullptr) { + return flightmode->requires_GPS(); + } switch (mode) { case AUTO: case GUIDED: @@ -366,6 +379,9 @@ bool Copter::mode_has_manual_throttle(control_mode_t mode) // arming_from_gcs should be set to true if the arming request comes from the ground station bool Copter::mode_allows_arming(control_mode_t mode, bool arming_from_gcs) { + if (flightmode != nullptr) { + return flightmode->allows_arming(arming_from_gcs); + } if (mode_has_manual_throttle(mode) || mode == LOITER || mode == ALT_HOLD || mode == POSHOLD || mode == DRIFT || mode == SPORT || mode == THROW || (arming_from_gcs && (mode == GUIDED || mode == GUIDED_NOGPS))) { return true; } @@ -377,6 +393,11 @@ void Copter::notify_flight_mode(control_mode_t mode) { AP_Notify::flags.flight_mode = mode; + if (flightmode != nullptr) { + AP_Notify::flags.autopilot_mode = flightmode->is_autopilot(); + notify.set_flight_mode_str(flightmode->name4()); + return; + } switch (mode) { case AUTO: case GUIDED: