From 2db09ba0f70d65d56530b681a5cc8d92127eecad Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 22 Mar 2016 13:13:34 +1100 Subject: [PATCH] Copter: FlightMode - convert GUIDED flight mode --- ArduCopter/Copter.cpp | 1 - ArduCopter/Copter.h | 28 +----- ArduCopter/FlightMode.h | 62 ++++++++++++ ArduCopter/GCS_Mavlink.cpp | 14 +-- ArduCopter/commands_logic.cpp | 8 +- ArduCopter/control_auto.cpp | 8 +- ArduCopter/control_avoid_adsb.cpp | 6 +- ArduCopter/control_guided.cpp | 150 ++++++++++++++-------------- ArduCopter/control_guided_nogps.cpp | 4 +- ArduCopter/flight_mode.cpp | 14 +-- ArduCopter/navigation.cpp | 8 +- ArduCopter/takeoff.cpp | 2 +- 12 files changed, 168 insertions(+), 137 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index b0f58f020c..40a2bd0967 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -33,7 +33,6 @@ Copter::Copter(void) home_bearing(0), home_distance(0), wp_distance(0), - guided_mode(Guided_TakeOff), rtl_state(RTL_InitialClimb), rtl_state_complete(false), smart_rtl_state(SmartRTL_PathFollow), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 6ec0ed0213..b064b3a2fc 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -392,9 +392,6 @@ private: float descend_max; // centimetres } nav_payload_place; - // Guided - GuidedMode guided_mode; // controls which controller is run (pos or vel) - // RTL RTLState rtl_state; // records state of rtl (initial climb, returning home, etc) bool rtl_state_complete; // set to true if the current state is completed @@ -841,31 +838,8 @@ private: float get_throttle_assist(float velz, float pilot_throttle_scaled); bool flip_init(bool ignore_checks); void flip_run(); - bool guided_init(bool ignore_checks); - bool guided_takeoff_start(float final_alt_above_home); - void guided_pos_control_start(); - void guided_vel_control_start(); - void guided_posvel_control_start(); - void guided_angle_control_start(); - bool guided_set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); - bool guided_set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); - void guided_set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); - bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); - void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads); - void guided_run(); - void guided_takeoff_run(); - void guided_pos_control_run(); - void guided_vel_control_run(); - void guided_posvel_control_run(); - void guided_angle_control_run(); - void guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des); - void guided_limit_clear(); - void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); - void guided_limit_init_time_and_pos(); - bool guided_limit_check(); bool guided_nogps_init(bool ignore_checks); void guided_nogps_run(); - void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle); bool land_init(bool ignore_checks); void land_run(); void land_gps_run(); @@ -1128,6 +1102,8 @@ private: Copter::FlightMode_CIRCLE flightmode_circle{*this, circle_nav}; + Copter::FlightMode_GUIDED flightmode_guided{*this}; + Copter::FlightMode_LOITER flightmode_loiter{*this}; #if FRAME_CONFIG == HELI_FRAME diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index c7a3fa4c7f..094b08ecfa 100644 --- a/ArduCopter/FlightMode.h +++ b/ArduCopter/FlightMode.h @@ -413,3 +413,65 @@ private: #endif }; + + + +class FlightMode_GUIDED : public FlightMode { + +public: + + FlightMode_GUIDED(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 { + if (from_gcs) { + return true; + } + return false; + }; + bool is_autopilot() const override { return true; } + + void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads); + bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); + bool set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); + void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); + bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); + + void limit_clear(); + void limit_init_time_and_pos(); + void limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); + bool limit_check(); + + bool takeoff_start(float final_alt_above_home); + + GuidedMode mode() { return guided_mode; } + + void angle_control_start(); + void angle_control_run(); + +protected: + + const char *name() const override { return "GUIDED"; } + const char *name4() const override { return "GUID"; } + +private: + + void pos_control_start(); + void vel_control_start(); + void posvel_control_start(); + void takeoff_run(); + void pos_control_run(); + void vel_control_run(); + void posvel_control_run(); + void set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des); + void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle); + + // controls which controller is run (pos or vel): + GuidedMode guided_mode = Guided_TakeOff; + +}; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index b867e7bf30..2a5b999809 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1364,7 +1364,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) use_yaw_rate = true; } - copter.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), + copter.flightmode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms, use_yaw_rate, packet.body_yaw_rate); break; @@ -1446,16 +1446,16 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) // send request if (!pos_ignore && !vel_ignore && acc_ignore) { - if (copter.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { + if (copter.flightmode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (pos_ignore && !vel_ignore && acc_ignore) { - copter.guided_set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.flightmode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); result = MAV_RESULT_ACCEPTED; } else if (!pos_ignore && vel_ignore && acc_ignore) { - if (copter.guided_set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { + if (copter.flightmode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; @@ -1548,16 +1548,16 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) } if (!pos_ignore && !vel_ignore && acc_ignore) { - if (copter.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { + if (copter.flightmode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (pos_ignore && !vel_ignore && acc_ignore) { - copter.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.flightmode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); result = MAV_RESULT_ACCEPTED; } else if (!pos_ignore && vel_ignore && acc_ignore) { - if (copter.guided_set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { + if (copter.flightmode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index beda87a810..1e2c6d520b 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -578,7 +578,7 @@ void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { if (cmd.p1 > 0) { // initialise guided limits - guided_limit_init_time_and_pos(); + flightmode_guided.limit_init_time_and_pos(); // set spline navigation target flightmode_auto.nav_guided_start(); @@ -649,7 +649,7 @@ void Copter::do_gripper(const AP_Mission::Mission_Command& cmd) // do_guided_limits - pass guided limits to guided controller void Copter::do_guided_limits(const AP_Mission::Mission_Command& cmd) { - guided_limit_set(cmd.p1 * 1000, // convert seconds to ms + flightmode_guided.limit_set(cmd.p1 * 1000, // convert seconds to ms cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm @@ -1005,7 +1005,7 @@ bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) } // check time and position limits - return guided_limit_check(); + return flightmode_guided.limit_check(); } #endif // NAV_GUIDED @@ -1104,7 +1104,7 @@ bool Copter::do_guided(const AP_Mission::Mission_Command& cmd) { // set wp_nav's destination Location_Class dest(cmd.content.location); - return guided_set_destination(dest); + return flightmode_guided.set_destination(dest); } case MAV_CMD_CONDITION_YAW: diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index ededd9d07d..3783b410d6 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -39,7 +39,7 @@ bool Copter::FlightMode_AUTO::init(bool ignore_checks) wp_nav->wp_and_spline_init(); // clear guided limits - _copter.guided_limit_clear(); + _copter.flightmode_guided.limit_clear(); // start/resume the mission (based on MIS_RESTART parameter) mission.start_or_resume(); @@ -534,10 +534,10 @@ void Copter::FlightMode_AUTO::nav_guided_start() _mode = Auto_NavGuided; // call regular guided flight mode initialisation - _copter.guided_init(true); + _copter.flightmode_guided.init(true); // initialise guided start time and position as reference for limit checking - _copter.guided_limit_init_time_and_pos(); + _copter.flightmode_guided.limit_init_time_and_pos(); } // auto_nav_guided_run - allows control by external navigation controller @@ -545,7 +545,7 @@ void Copter::FlightMode_AUTO::nav_guided_start() void Copter::FlightMode_AUTO::nav_guided_run() { // call regular guided flight mode run function - _copter.guided_run(); + _copter.flightmode_guided.run(); } #endif // NAV_GUIDED diff --git a/ArduCopter/control_avoid_adsb.cpp b/ArduCopter/control_avoid_adsb.cpp index 1dcdeb0597..c54b1c4ffd 100644 --- a/ArduCopter/control_avoid_adsb.cpp +++ b/ArduCopter/control_avoid_adsb.cpp @@ -13,7 +13,7 @@ bool Copter::avoid_adsb_init(const bool ignore_checks) { // re-use guided mode - return guided_init(ignore_checks); + return flightmode_guided.init(ignore_checks); } bool Copter::avoid_adsb_set_velocity(const Vector3f& velocity_neu) @@ -24,7 +24,7 @@ bool Copter::avoid_adsb_set_velocity(const Vector3f& velocity_neu) } // re-use guided mode's velocity controller - guided_set_velocity(velocity_neu); + flightmode_guided.set_velocity(velocity_neu); return true; } @@ -34,5 +34,5 @@ void Copter::avoid_adsb_run() // re-use guided mode's velocity controller // Note: this is safe from interference from GCSs and companion computer's whose guided mode // position and velocity requests will be ignored while the vehicle is not in guided mode - guided_run(); + flightmode_guided.run(); } diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 81dcc5b332..1efca123fb 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -36,13 +36,13 @@ struct Guided_Limit { } guided_limit; // guided_init - initialise guided controller -bool Copter::guided_init(bool ignore_checks) +bool Copter::FlightMode_GUIDED::init(bool ignore_checks) { - if (position_ok() || ignore_checks) { + if (_copter.position_ok() || ignore_checks) { // initialise yaw - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false)); // start in position control mode - guided_pos_control_start(); + pos_control_start(); return true; }else{ return false; @@ -51,17 +51,17 @@ bool Copter::guided_init(bool ignore_checks) // guided_takeoff_start - initialises waypoint controller to implement take-off -bool Copter::guided_takeoff_start(float final_alt_above_home) +bool Copter::FlightMode_GUIDED::takeoff_start(float final_alt_above_home) { guided_mode = Guided_TakeOff; // initialise wpnav destination - Location_Class target_loc = current_loc; + Location_Class target_loc = _copter.current_loc; target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME); if (!wp_nav->set_wp_destination(target_loc)) { // failure to set destination can only be because of missing terrain data - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); + _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); // failure is propagated to GCS with NAK return false; } @@ -73,13 +73,13 @@ bool Copter::guided_takeoff_start(float final_alt_above_home) set_throttle_takeoff(); // get initial alt for WP_NAVALT_MIN - auto_takeoff_set_start_alt(); - + _copter.auto_takeoff_set_start_alt(); + return true; } // initialise guided mode's position controller -void Copter::guided_pos_control_start() +void Copter::FlightMode_GUIDED::pos_control_start() { // set to position control mode guided_mode = Guided_WP; @@ -95,11 +95,11 @@ void Copter::guided_pos_control_start() wp_nav->set_wp_destination(stopping_point, false); // initialise yaw - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false)); } // initialise guided mode's velocity controller -void Copter::guided_vel_control_start() +void Copter::FlightMode_GUIDED::vel_control_start() { // set guided_mode to velocity controller guided_mode = Guided_Velocity; @@ -118,7 +118,7 @@ void Copter::guided_vel_control_start() } // initialise guided mode's posvel controller -void Copter::guided_posvel_control_start() +void Copter::FlightMode_GUIDED::posvel_control_start() { // set guided_mode to velocity controller guided_mode = Guided_PosVel; @@ -146,7 +146,7 @@ void Copter::guided_posvel_control_start() } // initialise guided mode's angle controller -void Copter::guided_angle_control_start() +void Copter::FlightMode_GUIDED::angle_control_start() { // set guided_mode to velocity controller guided_mode = Guided_Angle; @@ -163,7 +163,7 @@ void Copter::guided_angle_control_start() // initialise targets guided_angle_state.update_time_ms = millis(); - guided_angle_state.roll_cd = ahrs.roll_sensor; + guided_angle_state.roll_cd = _copter.ahrs.roll_sensor; guided_angle_state.pitch_cd = ahrs.pitch_sensor; guided_angle_state.yaw_cd = ahrs.yaw_sensor; guided_angle_state.climb_rate_cms = 0.0f; @@ -177,49 +177,49 @@ void Copter::guided_angle_control_start() // guided_set_destination - sets guided mode's target destination // Returns true if the fence is enabled and guided waypoint is within the fence // else return false if the waypoint is outside the fence -bool Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +bool Copter::FlightMode_GUIDED::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // ensure we are in position control mode if (guided_mode != Guided_WP) { - guided_pos_control_start(); + pos_control_start(); } #if AC_FENCE == ENABLED // reject destination if outside the fence Location_Class dest_loc(destination); - if (!fence.check_destination_within_fence(dest_loc)) { - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); + if (!_copter.fence.check_destination_within_fence(dest_loc)) { + _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } #endif // set yaw state - guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); // no need to check return status because terrain data is not used wp_nav->set_wp_destination(destination, false); // log target - Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); + _copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); return true; } // sets guided mode's target from a Location object // returns false if destination could not be set (probably caused by missing terrain data) // or if the fence is enabled and guided waypoint is outside the fence -bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +bool Copter::FlightMode_GUIDED::set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // ensure we are in position control mode if (guided_mode != Guided_WP) { - guided_pos_control_start(); + pos_control_start(); } #if AC_FENCE == ENABLED // reject destination outside the fence. // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails - if (!fence.check_destination_within_fence(dest_loc)) { - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); + if (!_copter.fence.check_destination_within_fence(dest_loc)) { + _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -227,76 +227,76 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw if (!wp_nav->set_wp_destination(dest_loc)) { // failure to set destination can only be because of missing terrain data - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); + _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); // failure is propagated to GCS with NAK return false; } // set yaw state - guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); // log target - Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); + _copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); return true; } // guided_set_velocity - sets guided mode's target velocity -void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +void Copter::FlightMode_GUIDED::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // check we are in velocity control mode if (guided_mode != Guided_Velocity) { - guided_vel_control_start(); + vel_control_start(); } // set yaw state - guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); // record velocity target guided_vel_target_cms = velocity; vel_update_time_ms = millis(); // log target - Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity); + _copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity); } // set guided mode posvel target -bool Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +bool Copter::FlightMode_GUIDED::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // check we are in velocity control mode if (guided_mode != Guided_PosVel) { - guided_posvel_control_start(); + posvel_control_start(); } #if AC_FENCE == ENABLED // reject destination if outside the fence Location_Class dest_loc(destination); - if (!fence.check_destination_within_fence(dest_loc)) { - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); + if (!_copter.fence.check_destination_within_fence(dest_loc)) { + _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } #endif // set yaw state - guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); + set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); posvel_update_time_ms = millis(); guided_pos_target_cm = destination; guided_vel_target_cms = velocity; - pos_control->set_pos_target(guided_pos_target_cm); + _copter.pos_control->set_pos_target(guided_pos_target_cm); // log target - Log_Write_GuidedTarget(guided_mode, destination, velocity); + _copter.Log_Write_GuidedTarget(guided_mode, destination, velocity); return true; } // set guided mode angle target -void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads) +void Copter::FlightMode_GUIDED::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads) { // check we are in velocity control mode if (guided_mode != Guided_Angle) { - guided_angle_control_start(); + angle_control_start(); } // convert quaternion to euler angles @@ -312,52 +312,52 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms, bool us // interpret positive climb rate as triggering take-off if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) { - set_auto_armed(true); + _copter.set_auto_armed(true); } // log target - Log_Write_GuidedTarget(guided_mode, + _copter.Log_Write_GuidedTarget(guided_mode, Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd), Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms)); } // guided_run - runs the guided controller // should be called at 100hz or more -void Copter::guided_run() +void Copter::FlightMode_GUIDED::run() { // call the correct auto controller switch (guided_mode) { case Guided_TakeOff: // run takeoff controller - guided_takeoff_run(); + takeoff_run(); break; case Guided_WP: // run position controller - guided_pos_control_run(); + pos_control_run(); break; case Guided_Velocity: // run velocity controller - guided_vel_control_run(); + vel_control_run(); break; case Guided_PosVel: // run position-velocity controller - guided_posvel_control_run(); + posvel_control_run(); break; case Guided_Angle: // run angle controller - guided_angle_control_run(); + angle_control_run(); break; } } // guided_takeoff_run - takeoff in guided mode // called by guided_run at 100hz or more -void Copter::guided_takeoff_run() +void Copter::FlightMode_GUIDED::takeoff_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { @@ -379,7 +379,7 @@ void Copter::guided_takeoff_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!_copter.failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } @@ -400,18 +400,18 @@ void Copter::guided_takeoff_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - failsafe_terrain_set_status(wp_nav->update_wpnav()); + _copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) - pos_control->update_z_controller(); + _copter.pos_control->update_z_controller(); // call attitude controller - auto_takeoff_attitude_run(target_yaw_rate); + _copter.auto_takeoff_attitude_run(target_yaw_rate); } // guided_pos_control_run - runs the guided position controller // called from guided_run -void Copter::guided_pos_control_run() +void Copter::FlightMode_GUIDED::pos_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { @@ -429,7 +429,7 @@ void Copter::guided_pos_control_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!_copter.failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { @@ -441,13 +441,13 @@ void Copter::guided_pos_control_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - failsafe_terrain_set_status(wp_nav->update_wpnav()); + _copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control->update_z_controller(); // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { + if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); } else if (auto_yaw_mode == AUTO_YAW_RATE) { @@ -461,7 +461,7 @@ void Copter::guided_pos_control_run() // guided_vel_control_run - runs the guided velocity controller // called from guided_run -void Copter::guided_vel_control_run() +void Copter::FlightMode_GUIDED::vel_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { @@ -481,7 +481,7 @@ void Copter::guided_vel_control_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!_copter.failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { @@ -496,20 +496,20 @@ void Copter::guided_vel_control_run() uint32_t tnow = millis(); if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { if (!pos_control->get_desired_velocity().is_zero()) { - guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f)); + set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f)); } if (auto_yaw_mode == AUTO_YAW_RATE) { set_auto_yaw_rate(0.0f); } } else { - guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms); + set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms); } // call velocity controller which includes z axis controller pos_control->update_vel_controller_xyz(ekfNavVelGainScaler); // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { + if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain()); } else if (auto_yaw_mode == AUTO_YAW_RATE) { @@ -523,7 +523,7 @@ void Copter::guided_vel_control_run() // guided_posvel_control_run - runs the guided spline controller // called from guided_run -void Copter::guided_posvel_control_run() +void Copter::FlightMode_GUIDED::posvel_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { @@ -545,7 +545,7 @@ void Copter::guided_posvel_control_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!_copter.failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { @@ -589,7 +589,7 @@ void Copter::guided_posvel_control_run() pos_control->update_z_controller(); // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { + if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain()); } else if (auto_yaw_mode == AUTO_YAW_RATE) { @@ -603,7 +603,7 @@ void Copter::guided_posvel_control_run() // guided_angle_control_run - runs the guided angle controller // called from guided_run -void Copter::guided_angle_control_run() +void Copter::FlightMode_GUIDED::angle_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) { @@ -625,7 +625,7 @@ void Copter::guided_angle_control_run() float roll_in = guided_angle_state.roll_cd; float pitch_in = guided_angle_state.pitch_cd; float total_in = norm(roll_in, pitch_in); - float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), aparm.angle_max); + float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), _copter.aparm.angle_max); if (total_in > angle_max) { float ratio = angle_max / total_in; roll_in *= ratio; @@ -667,7 +667,7 @@ void Copter::guided_angle_control_run() } // helper function to update position controller's desired velocity while respecting acceleration limits -void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des) +void Copter::FlightMode_GUIDED::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des) { // get current desired velocity Vector3f curr_vel_des = pos_control->get_desired_velocity(); @@ -691,7 +691,7 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto #if AC_AVOID_ENABLED // limit the velocity to prevent fence violations - avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des); + _copter.avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des); // get avoidance adjusted climb rate curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z); #endif @@ -701,7 +701,7 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto } // helper function to set yaw state and targets -void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) +void Copter::FlightMode_GUIDED::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) { if (use_yaw) { set_auto_yaw_look_at_heading(yaw_cd / 100.0f, 0.0f, 0, relative_angle); @@ -713,7 +713,7 @@ void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, // Guided Limit code // guided_limit_clear - clear/turn off guided limits -void Copter::guided_limit_clear() +void Copter::FlightMode_GUIDED::limit_clear() { guided_limit.timeout_ms = 0; guided_limit.alt_min_cm = 0.0f; @@ -722,7 +722,7 @@ void Copter::guided_limit_clear() } // guided_limit_set - set guided timeout and movement limits -void Copter::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) +void Copter::FlightMode_GUIDED::limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm) { guided_limit.timeout_ms = timeout_ms; guided_limit.alt_min_cm = alt_min_cm; @@ -732,7 +732,7 @@ void Copter::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_m // guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking // only called from AUTO mode's auto_nav_guided_start function -void Copter::guided_limit_init_time_and_pos() +void Copter::FlightMode_GUIDED::limit_init_time_and_pos() { // initialise start time guided_limit.start_time = AP_HAL::millis(); @@ -743,7 +743,7 @@ void Copter::guided_limit_init_time_and_pos() // guided_limit_check - returns true if guided mode has breached a limit // used when guided is invoked from the NAV_GUIDED_ENABLE mission command -bool Copter::guided_limit_check() +bool Copter::FlightMode_GUIDED::limit_check() { // check if we have passed the timeout if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time >= guided_limit.timeout_ms)) { diff --git a/ArduCopter/control_guided_nogps.cpp b/ArduCopter/control_guided_nogps.cpp index a56785c94f..3fe9ce6139 100644 --- a/ArduCopter/control_guided_nogps.cpp +++ b/ArduCopter/control_guided_nogps.cpp @@ -8,7 +8,7 @@ bool Copter::guided_nogps_init(bool ignore_checks) { // start in angle control mode - guided_angle_control_start(); + flightmode_guided.angle_control_start(); return true; } @@ -17,6 +17,6 @@ bool Copter::guided_nogps_init(bool ignore_checks) void Copter::guided_nogps_run() { // run angle controller - guided_angle_control_run(); + flightmode_guided.angle_control_run(); } diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 7e201704b4..a16074a20f 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -82,7 +82,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) break; case GUIDED: - success = guided_init(ignore_checks); + success = flightmode_guided.init(ignore_checks); + if (success) { + flightmode = &flightmode_guided; + } break; case LAND: @@ -203,10 +206,6 @@ void Copter::update_flight_mode() switch (control_mode) { - case GUIDED: - guided_run(); - break; - case LAND: land_run(); break; @@ -325,7 +324,6 @@ bool Copter::mode_requires_GPS() return flightmode->requires_GPS(); } switch (control_mode) { - case GUIDED: case RTL: case SMART_RTL: case DRIFT: @@ -376,7 +374,6 @@ void Copter::notify_flight_mode() return; } switch (control_mode) { - case GUIDED: case RTL: case AVOID_ADSB: case GUIDED_NOGPS: @@ -393,9 +390,6 @@ void Copter::notify_flight_mode() // set flight mode string switch (control_mode) { - case GUIDED: - notify.set_flight_mode_str("GUID"); - break; case RTL: notify.set_flight_mode_str("RTL "); break; diff --git a/ArduCopter/navigation.cpp b/ArduCopter/navigation.cpp index afdfb34bb2..2194946b7c 100644 --- a/ArduCopter/navigation.cpp +++ b/ArduCopter/navigation.cpp @@ -37,11 +37,11 @@ void Copter::calc_wp_distance() break; case GUIDED: - if (guided_mode == Guided_WP) { + if (flightmode_guided.mode() == Guided_WP) { wp_distance = wp_nav->get_wp_distance_to_destination(); break; } - if (guided_mode == Guided_PosVel) { + if (flightmode_guided.mode() == Guided_PosVel) { wp_distance = pos_control->get_distance_to_target(); break; } @@ -69,11 +69,11 @@ void Copter::calc_wp_bearing() break; case GUIDED: - if (guided_mode == Guided_WP) { + if (flightmode_guided.mode() == Guided_WP) { wp_bearing = wp_nav->get_wp_bearing_to_destination(); break; } - if (guided_mode == Guided_PosVel) { + if (flightmode_guided.mode() == Guided_PosVel) { wp_bearing = pos_control->get_bearing_to_target(); break; } diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index aaf8247d22..bb67c00e49 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -36,7 +36,7 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) switch(control_mode) { case GUIDED: - if (guided_takeoff_start(takeoff_alt_cm)) { + if (flightmode_guided.takeoff_start(takeoff_alt_cm)) { set_auto_armed(true); return true; }