diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index fc56a50383..f303d8778a 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), - auto_mode(Auto_TakeOff), guided_mode(Guided_TakeOff), rtl_state(RTL_InitialClimb), rtl_state_complete(false), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index d52299c364..9784ef90b3 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -392,9 +392,6 @@ private: float descend_max; // centimetres } nav_payload_place; - // Auto - AutoMode auto_mode; // controls which auto controller is run - // Guided GuidedMode guided_mode; // controls which controller is run (pos or vel) @@ -796,34 +793,7 @@ private: MAV_RESULT mavlink_compassmot(mavlink_channel_t chan); void delay(uint32_t ms); void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); - bool auto_init(bool ignore_checks); - void auto_run(); - void auto_takeoff_start(const Location& dest_loc); - void auto_takeoff_run(); - void auto_wp_start(const Vector3f& destination); - void auto_wp_start(const Location_Class& dest_loc); - void auto_wp_run(); - void auto_spline_run(); - void auto_land_start(); - void auto_land_start(const Vector3f& destination); - void auto_land_run(); void do_payload_place(const AP_Mission::Mission_Command& cmd); - void auto_payload_place_start(); - void auto_payload_place_start(const Vector3f& destination); - void auto_payload_place_run(); - bool auto_payload_place_run_should_run(); - void auto_payload_place_run_loiter(); - void auto_payload_place_run_descend(); - void auto_payload_place_run_release(); - void auto_rtl_start(); - void auto_rtl_run(); - void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m); - void auto_circle_start(); - void auto_circle_run(); - void auto_nav_guided_start(); - void auto_nav_guided_run(); - bool auto_loiter_start(); - void auto_loiter_run(); uint8_t get_default_auto_yaw_mode(bool rtl); void set_auto_yaw_mode(uint8_t yaw_mode); void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle); @@ -1167,6 +1137,8 @@ private: Copter::FlightMode_ALTHOLD flightmode_althold{*this}; + Copter::FlightMode_AUTO flightmode_auto{*this, mission, circle_nav}; + #if FRAME_CONFIG == HELI_FRAME Copter::FlightMode_STABILIZE_Heli flightmode_stabilize{*this}; #else diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index 2efa20e9e9..784925272f 100644 --- a/ArduCopter/FlightMode.h +++ b/ArduCopter/FlightMode.h @@ -270,3 +270,74 @@ private: }; #endif + + + +class FlightMode_AUTO : public FlightMode { + +public: + + FlightMode_AUTO(Copter &copter, AP_Mission &_mission, AC_Circle *& _circle_nav) : + Copter::FlightMode(copter), + mission(_mission), + circle_nav(_circle_nav) + { } + + virtual bool init(bool ignore_checks) override; + virtual void run() override; // should be called at 100hz or more + + virtual bool is_autopilot() const override { return true; } + virtual bool requires_GPS() const override { return true; } + virtual bool has_manual_throttle() const override { return false; } + virtual bool allows_arming(bool from_gcs) const override { return false; }; + + // Auto + AutoMode mode() { return _mode; } + + bool loiter_start(); + void rtl_start(); + void takeoff_start(const Location& dest_loc); + void wp_start(const Vector3f& destination); + void wp_start(const Location_Class& dest_loc); + void land_start(); + void land_start(const Vector3f& destination); + void circle_movetoedge_start(const Location_Class &circle_center, float radius_m); + void circle_start(); + void spline_start(const Vector3f& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Vector3f& next_spline_destination); + void spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination); + void nav_guided_start(); + + bool landing_gear_should_be_deployed(); + + void payload_place_start(); + +protected: + + const char *name() const override { return "AUTO"; } + const char *name4() const override { return "AUTO"; } + +// void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); + +private: + + void takeoff_run(); + void wp_run(); + void spline_run(); + void land_run(); + void rtl_run(); + void circle_run(); + void nav_guided_run(); + void loiter_run(); + + void payload_place_start(const Vector3f& destination); + void payload_place_run(); + bool payload_place_run_should_run(); + void payload_place_run_loiter(); + void payload_place_run_descend(); + void payload_place_run_release(); + + AutoMode _mode = Auto_TakeOff; // controls which auto controller is run + + AP_Mission &mission; + AC_Circle *&circle_nav; +}; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 9d4b2a0bf9..b867e7bf30 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1335,7 +1335,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) mavlink_msg_set_attitude_target_decode(msg, &packet); // exit if vehicle is not in Guided mode or Auto-Guided mode - if ((copter.control_mode != GUIDED) && (copter.control_mode != GUIDED_NOGPS) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) { + if ((copter.control_mode != GUIDED) && (copter.control_mode != GUIDED_NOGPS) && !(copter.control_mode == AUTO && copter.flightmode_auto.mode() == Auto_NavGuided)) { break; } @@ -1377,7 +1377,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) mavlink_msg_set_position_target_local_ned_decode(msg, &packet); // exit if vehicle is not in Guided mode or Auto-Guided mode - if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) { + if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.flightmode_auto.mode() == Auto_NavGuided)) { break; } @@ -1474,7 +1474,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) mavlink_msg_set_position_target_global_int_decode(msg, &packet); // exit if vehicle is not in Guided mode or Auto-Guided mode - if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.auto_mode == Auto_NavGuided)) { + if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.flightmode_auto.mode() == Auto_NavGuided)) { break; } diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 8e2c82886a..beda87a810 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -292,7 +292,7 @@ void Copter::exit_mission() // if we are not on the ground switch to loiter or land if(!ap.land_complete) { // try to enter loiter but if that fails land - if(!auto_loiter_start()) { + if(!flightmode_auto.loiter_start()) { set_mode(LAND, MODE_REASON_MISSION_END); } }else{ @@ -309,7 +309,7 @@ void Copter::exit_mission() void Copter::do_RTL(void) { // start rtl in auto flight mode - auto_rtl_start(); + flightmode_auto.rtl_start(); } /********************************************************************************/ @@ -320,7 +320,7 @@ void Copter::do_RTL(void) void Copter::do_takeoff(const AP_Mission::Mission_Command& cmd) { // Set wp navigation target to safe altitude above current position - auto_takeoff_start(cmd.content.location); + flightmode_auto.takeoff_start(cmd.content.location); } // do_nav_wp - initiate move to next waypoint @@ -350,7 +350,7 @@ void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd) loiter_time_max = cmd.p1; // Set wp navigation target - auto_wp_start(target_loc); + flightmode_auto.wp_start(target_loc); // if no delay set the waypoint as "fast" if (loiter_time_max == 0 ) { @@ -391,13 +391,13 @@ void Copter::do_land(const AP_Mission::Mission_Command& cmd) Location_Class target_loc = terrain_adjusted_location(cmd); - auto_wp_start(target_loc); + flightmode_auto.wp_start(target_loc); }else{ // set landing state land_state = LandStateType_Descending; // initialise landing controller - auto_land_start(); + flightmode_auto.land_start(); } } @@ -411,12 +411,12 @@ void Copter::do_payload_place(const AP_Mission::Mission_Command& cmd) Location_Class target_loc = terrain_adjusted_location(cmd); - auto_wp_start(target_loc); + flightmode_auto.wp_start(target_loc); } else { nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; // initialise placing controller - auto_payload_place_start(); + flightmode_auto.payload_place_start(); } nav_payload_place.descend_max = cmd.p1; } @@ -452,7 +452,7 @@ void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) } // start way point navigator and provide it the desired location - auto_wp_start(target_loc); + flightmode_auto.wp_start(target_loc); } // do_circle - initiate moving in a circle @@ -484,7 +484,7 @@ void Copter::do_circle(const AP_Mission::Mission_Command& cmd) uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge - auto_circle_movetoedge_start(circle_center, circle_radius_m); + flightmode_auto.circle_movetoedge_start(circle_center, circle_radius_m); } // do_loiter_time - initiate loitering at a point for a given time period @@ -569,7 +569,7 @@ void Copter::do_spline_wp(const AP_Mission::Mission_Command& cmd) } // set spline navigation target - auto_spline_start(target_loc, stopped_at_start, seg_end_type, next_loc); + flightmode_auto.spline_start(target_loc, stopped_at_start, seg_end_type, next_loc); } #if NAV_GUIDED == ENABLED @@ -581,7 +581,7 @@ void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) guided_limit_init_time_and_pos(); // set spline navigation target - auto_nav_guided_start(); + flightmode_auto.nav_guided_start(); } } #endif // NAV_GUIDED @@ -703,7 +703,7 @@ bool Copter::verify_land() Vector3f dest = wp_nav->get_wp_destination(); // initialise landing controller - auto_land_start(dest); + flightmode_auto.land_start(dest); // advance to next state land_state = LandStateType_Descending; @@ -774,7 +774,6 @@ bool Copter::verify_payload_place() return false; } // we're there; set loiter target - auto_payload_place_start(wp_nav->get_wp_destination()); nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; FALLTHROUGH; case PayloadPlaceStateType_Calibrating_Hover_Start: @@ -867,7 +866,7 @@ bool Copter::verify_payload_place() case PayloadPlaceStateType_Ascending_Start: { Location_Class target_loc = inertial_nav.get_position(); target_loc.alt = nav_payload_place.descend_start_altitude; - auto_wp_start(target_loc); + flightmode_auto.wp_start(target_loc); nav_payload_place.state = PayloadPlaceStateType_Ascending; } FALLTHROUGH; @@ -940,7 +939,7 @@ bool Copter::verify_loiter_time() bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd) { // check if we've reached the edge - if (auto_mode == Auto_CircleMoveToEdge) { + if (flightmode_auto.mode() == Auto_CircleMoveToEdge) { if (wp_nav->reached_wp_destination()) { Vector3f curr_pos = inertial_nav.get_position(); Vector3f circle_center = pv_location_to_vector(cmd.content.location); @@ -957,7 +956,7 @@ bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd) } // start circling - auto_circle_start(); + flightmode_auto.circle_start(); } return false; } @@ -1094,7 +1093,7 @@ bool Copter::verify_yaw() bool Copter::do_guided(const AP_Mission::Mission_Command& cmd) { // only process guided waypoint if we are in guided mode - if (control_mode != GUIDED && !(control_mode == AUTO && auto_mode == Auto_NavGuided)) { + if (control_mode != GUIDED && !(control_mode == AUTO && flightmode_auto.mode() == Auto_NavGuided)) { return false; } diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 2b8df5fcfd..ededd9d07d 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -18,10 +18,10 @@ */ // auto_init - initialise auto controller -bool Copter::auto_init(bool ignore_checks) +bool Copter::FlightMode_AUTO::init(bool ignore_checks) { - if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { - auto_mode = Auto_Loiter; + if ((_copter.position_ok() && mission.num_commands() > 1) || ignore_checks) { + _mode = Auto_Loiter; // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips) if (motors->armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) { @@ -31,7 +31,7 @@ bool Copter::auto_init(bool ignore_checks) // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check - if (auto_yaw_mode == AUTO_YAW_ROI) { + if (_copter.auto_yaw_mode == AUTO_YAW_ROI) { set_auto_yaw_mode(AUTO_YAW_HOLD); } @@ -39,7 +39,7 @@ bool Copter::auto_init(bool ignore_checks) wp_nav->wp_and_spline_init(); // clear guided limits - guided_limit_clear(); + _copter.guided_limit_clear(); // start/resume the mission (based on MIS_RESTART parameter) mission.start_or_resume(); @@ -52,76 +52,76 @@ bool Copter::auto_init(bool ignore_checks) // auto_run - runs the auto controller // should be called at 100hz or more // relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands -void Copter::auto_run() +void Copter::FlightMode_AUTO::run() { // call the correct auto controller - switch (auto_mode) { + switch (_mode) { case Auto_TakeOff: - auto_takeoff_run(); + takeoff_run(); break; case Auto_WP: case Auto_CircleMoveToEdge: - auto_wp_run(); + wp_run(); break; case Auto_Land: - auto_land_run(); + land_run(); break; case Auto_RTL: - auto_rtl_run(); + rtl_run(); break; case Auto_Circle: - auto_circle_run(); + circle_run(); break; case Auto_Spline: - auto_spline_run(); + spline_run(); break; case Auto_NavGuided: #if NAV_GUIDED == ENABLED - auto_nav_guided_run(); + nav_guided_run(); #endif break; case Auto_Loiter: - auto_loiter_run(); + loiter_run(); break; case Auto_NavPayloadPlace: - auto_payload_place_run(); + payload_place_run(); break; } } // auto_takeoff_start - initialises waypoint controller to implement take-off -void Copter::auto_takeoff_start(const Location& dest_loc) +void Copter::FlightMode_AUTO::takeoff_start(const Location& dest_loc) { - auto_mode = Auto_TakeOff; + _mode = Auto_TakeOff; // convert location to class Location_Class dest(dest_loc); // set horizontal target - dest.lat = current_loc.lat; - dest.lng = current_loc.lng; + dest.lat = _copter.current_loc.lat; + dest.lng = _copter.current_loc.lng; // get altitude target int32_t alt_target; if (!dest.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target)) { // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data - Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); + _copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); // fall back to altitude above current altitude - alt_target = current_loc.alt + dest.alt; + alt_target = _copter.current_loc.alt + dest.alt; } // sanity check target - if (alt_target < current_loc.alt) { - dest.set_alt_cm(current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME); + if (alt_target < _copter.current_loc.alt) { + dest.set_alt_cm(_copter.current_loc.alt, Location_Class::ALT_FRAME_ABOVE_HOME); } // Note: if taking off from below home this could cause a climb to an unexpectedly high altitude if (alt_target < 100) { @@ -131,7 +131,7 @@ void Copter::auto_takeoff_start(const Location& dest_loc) // set waypoint controller target if (!wp_nav->set_wp_destination(dest)) { // failure to set destination can only be because of missing terrain data - failsafe_terrain_on_event(); + _copter.failsafe_terrain_on_event(); return; } @@ -142,12 +142,12 @@ void Copter::auto_takeoff_start(const Location& dest_loc) set_throttle_takeoff(); // get initial alt for WP_NAVALT_MIN - auto_takeoff_set_start_alt(); + _copter.auto_takeoff_set_start_alt(); } // auto_takeoff_run - takeoff in auto mode // called by auto_run at 100hz or more -void Copter::auto_takeoff_run() +void Copter::FlightMode_AUTO::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()) { @@ -169,7 +169,7 @@ void Copter::auto_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()); } @@ -190,52 +190,52 @@ void Copter::auto_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(); // call attitude controller - auto_takeoff_attitude_run(target_yaw_rate); + _copter.auto_takeoff_attitude_run(target_yaw_rate); } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void Copter::auto_wp_start(const Vector3f& destination) +void Copter::FlightMode_AUTO::wp_start(const Vector3f& destination) { - auto_mode = Auto_WP; + _mode = Auto_WP; // initialise wpnav (no need to check return status because terrain data is not used) wp_nav->set_wp_destination(destination, false); // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (auto_yaw_mode != AUTO_YAW_ROI) { - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + if (_copter.auto_yaw_mode != AUTO_YAW_ROI) { + set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false)); } } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination -void Copter::auto_wp_start(const Location_Class& dest_loc) +void Copter::FlightMode_AUTO::wp_start(const Location_Class& dest_loc) { - auto_mode = Auto_WP; + _mode = Auto_WP; // send target to waypoint controller if (!wp_nav->set_wp_destination(dest_loc)) { // failure to set destination can only be because of missing terrain data - failsafe_terrain_on_event(); + _copter.failsafe_terrain_on_event(); return; } // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (auto_yaw_mode != AUTO_YAW_ROI) { - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + if (_copter.auto_yaw_mode != AUTO_YAW_ROI) { + set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false)); } } // auto_wp_run - runs the auto waypoint controller // called by auto_run at 100hz or more -void Copter::auto_wp_run() +void Copter::FlightMode_AUTO::wp_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()) { @@ -256,7 +256,7 @@ void Copter::auto_wp_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)) { @@ -268,13 +268,13 @@ void Copter::auto_wp_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{ @@ -285,29 +285,29 @@ void Copter::auto_wp_run() // auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller // seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided -void Copter::auto_spline_start(const Location_Class& destination, bool stopped_at_start, +void Copter::FlightMode_AUTO::spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination) { - auto_mode = Auto_Spline; + _mode = Auto_Spline; // initialise wpnav if (!wp_nav->set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) { // failure to set destination can only be because of missing terrain data - failsafe_terrain_on_event(); + _copter.failsafe_terrain_on_event(); return; } // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (auto_yaw_mode != AUTO_YAW_ROI) { - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + if (_copter.auto_yaw_mode != AUTO_YAW_ROI) { + set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false)); } } // auto_spline_run - runs the auto spline controller // called by auto_run at 100hz or more -void Copter::auto_spline_run() +void Copter::FlightMode_AUTO::spline_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()) { @@ -328,7 +328,7 @@ void Copter::auto_spline_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!_copter.failsafe.radio) { // get pilot's desired yaw rat target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { @@ -346,7 +346,7 @@ void Copter::auto_spline_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(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ @@ -356,20 +356,20 @@ void Copter::auto_spline_run() } // auto_land_start - initialises controller to implement a landing -void Copter::auto_land_start() +void Copter::FlightMode_AUTO::land_start() { // set target to stopping point Vector3f stopping_point; wp_nav->get_loiter_stopping_point_xy(stopping_point); // call location specific land start function - auto_land_start(stopping_point); + land_start(stopping_point); } // auto_land_start - initialises controller to implement a landing -void Copter::auto_land_start(const Vector3f& destination) +void Copter::FlightMode_AUTO::land_start(const Vector3f& destination) { - auto_mode = Auto_Land; + _mode = Auto_Land; // initialise loiter target destination wp_nav->init_loiter_target(destination); @@ -386,7 +386,7 @@ void Copter::auto_land_start(const Vector3f& destination) // auto_land_run - lands in auto mode // called by auto_run at 100hz or more -void Copter::auto_land_run() +void Copter::FlightMode_AUTO::land_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()) { @@ -407,37 +407,58 @@ void Copter::auto_land_run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - land_run_horizontal_control(); - land_run_vertical_control(); + _copter.land_run_horizontal_control(); + _copter.land_run_vertical_control(); +} + +bool Copter::FlightMode_AUTO::landing_gear_should_be_deployed() +{ + switch(_mode) { + case Auto_Land: + return true; + case Auto_RTL: + switch(_copter.rtl_state) { + case RTL_LoiterAtHome: + case RTL_Land: + case RTL_FinalDescent: + return true; + default: + return false; + } + return false; + default: + return false; + } + return false; } // auto_rtl_start - initialises RTL in AUTO flight mode -void Copter::auto_rtl_start() +void Copter::FlightMode_AUTO::rtl_start() { - auto_mode = Auto_RTL; + _mode = Auto_RTL; // call regular rtl flight mode initialisation and ask it to ignore checks - rtl_init(true); + _copter.rtl_init(true); } // auto_rtl_run - rtl in AUTO flight mode // called by auto_run at 100hz or more -void Copter::auto_rtl_run() +void Copter::FlightMode_AUTO::rtl_run() { // call regular rtl flight mode run function - rtl_run(false); + _copter.rtl_run(false); } // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location // we assume the caller has performed all required GPS_ok checks -void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m) +void Copter::FlightMode_AUTO::circle_movetoedge_start(const Location_Class &circle_center, float radius_m) { // convert location to vector from ekf origin Vector3f circle_center_neu; if (!circle_center.get_vector_from_origin_NEU(circle_center_neu)) { // default to current position and log error circle_center_neu = inertial_nav.get_position(); - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT); + _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT); } circle_nav->set_center(circle_center_neu); @@ -454,7 +475,7 @@ void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, f // if more than 3m then fly to edge if (dist_to_edge > 300.0f) { // set the state to move to the edge of the circle - auto_mode = Auto_CircleMoveToEdge; + _mode = Auto_CircleMoveToEdge; // convert circle_edge_neu to Location_Class Location_Class circle_edge(circle_edge_neu); @@ -465,28 +486,28 @@ void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, f // initialise wpnav to move to edge of circle if (!wp_nav->set_wp_destination(circle_edge)) { // failure to set destination can only be because of missing terrain data - failsafe_terrain_on_event(); + _copter.failsafe_terrain_on_event(); } // if we are outside the circle, point at the edge, otherwise hold yaw const Vector3f &curr_pos = inertial_nav.get_position(); float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y); if (dist_to_center > circle_nav->get_radius() && dist_to_center > 500) { - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(false)); } else { // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle set_auto_yaw_mode(AUTO_YAW_HOLD); } } else { - auto_circle_start(); + circle_start(); } } // auto_circle_start - initialises controller to fly a circle in AUTO flight mode // assumes that circle_nav object has already been initialised with circle center and radius -void Copter::auto_circle_start() +void Copter::FlightMode_AUTO::circle_start() { - auto_mode = Auto_Circle; + _mode = Auto_Circle; // initialise circle controller circle_nav->init(circle_nav->get_center()); @@ -494,7 +515,7 @@ void Copter::auto_circle_start() // auto_circle_run - circle in AUTO flight mode // called by auto_run at 100hz or more -void Copter::auto_circle_run() +void Copter::FlightMode_AUTO::circle_run() { // call circle controller circle_nav->update(); @@ -508,35 +529,35 @@ void Copter::auto_circle_run() #if NAV_GUIDED == ENABLED // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode -void Copter::auto_nav_guided_start() +void Copter::FlightMode_AUTO::nav_guided_start() { - auto_mode = Auto_NavGuided; + _mode = Auto_NavGuided; // call regular guided flight mode initialisation - guided_init(true); + _copter.guided_init(true); // initialise guided start time and position as reference for limit checking - guided_limit_init_time_and_pos(); + _copter.guided_limit_init_time_and_pos(); } // auto_nav_guided_run - allows control by external navigation controller // called by auto_run at 100hz or more -void Copter::auto_nav_guided_run() +void Copter::FlightMode_AUTO::nav_guided_run() { // call regular guided flight mode run function - guided_run(); + _copter.guided_run(); } #endif // NAV_GUIDED // auto_loiter_start - initialises loitering in auto mode // returns success/failure because this can be called by exit_mission -bool Copter::auto_loiter_start() +bool Copter::FlightMode_AUTO::loiter_start() { // return failure if GPS is bad - if (!position_ok()) { + if (!_copter.position_ok()) { return false; } - auto_mode = Auto_Loiter; + _mode = Auto_Loiter; // calculate stopping point Vector3f stopping_point; @@ -553,7 +574,7 @@ bool Copter::auto_loiter_start() // auto_loiter_run - loiter in AUTO flight mode // called by auto_run at 100hz or more -void Copter::auto_loiter_run() +void Copter::FlightMode_AUTO::loiter_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { @@ -571,7 +592,7 @@ void Copter::auto_loiter_run() // accept pilot input of yaw float target_yaw_rate = 0; - if(!failsafe.radio) { + if(!_copter.failsafe.radio) { target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } @@ -579,7 +600,7 @@ void Copter::auto_loiter_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint and z-axis position controller - failsafe_terrain_set_status(wp_nav->update_wpnav()); + _copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); pos_control->update_z_controller(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); @@ -771,22 +792,22 @@ float Copter::get_auto_yaw_rate_cds(void) } // auto_payload_place_start - initialises controller to implement a placing -void Copter::auto_payload_place_start() +void Copter::FlightMode_AUTO::payload_place_start() { // set target to stopping point Vector3f stopping_point; wp_nav->get_loiter_stopping_point_xy(stopping_point); // call location specific place start function - auto_payload_place_start(stopping_point); + payload_place_start(stopping_point); } // auto_payload_place_start - initialises controller to implement placement of a load -void Copter::auto_payload_place_start(const Vector3f& destination) +void Copter::FlightMode_AUTO::payload_place_start(const Vector3f& destination) { - auto_mode = Auto_NavPayloadPlace; - nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; + _mode = Auto_NavPayloadPlace; + _copter.nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; // initialise loiter target destination wp_nav->init_loiter_target(destination); @@ -801,7 +822,7 @@ void Copter::auto_payload_place_start(const Vector3f& destination) set_auto_yaw_mode(AUTO_YAW_HOLD); } -bool Copter::auto_payload_place_run_should_run() +bool Copter::FlightMode_AUTO::payload_place_run_should_run() { // muts be armed if (!motors->armed()) { @@ -825,9 +846,9 @@ bool Copter::auto_payload_place_run_should_run() // auto_payload_place_run - places an object in auto mode // called by auto_run at 100hz or more -void Copter::auto_payload_place_run() +void Copter::FlightMode_AUTO::payload_place_run() { - if (!auto_payload_place_run_should_run()) { + if (!payload_place_run_should_run()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); @@ -845,28 +866,28 @@ void Copter::auto_payload_place_run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - switch (nav_payload_place.state) { + switch (_copter.nav_payload_place.state) { case PayloadPlaceStateType_FlyToLocation: case PayloadPlaceStateType_Calibrating_Hover_Start: case PayloadPlaceStateType_Calibrating_Hover: - return auto_payload_place_run_loiter(); + return payload_place_run_loiter(); case PayloadPlaceStateType_Descending_Start: case PayloadPlaceStateType_Descending: - return auto_payload_place_run_descend(); + return payload_place_run_descend(); case PayloadPlaceStateType_Releasing_Start: case PayloadPlaceStateType_Releasing: case PayloadPlaceStateType_Released: case PayloadPlaceStateType_Ascending_Start: case PayloadPlaceStateType_Ascending: case PayloadPlaceStateType_Done: - return auto_payload_place_run_loiter(); + return payload_place_run_loiter(); } } -void Copter::auto_payload_place_run_loiter() +void Copter::FlightMode_AUTO::payload_place_run_loiter() { // loiter... - land_run_horizontal_control(); + _copter.land_run_horizontal_control(); // run loiter controller wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); @@ -879,8 +900,8 @@ void Copter::auto_payload_place_run_loiter() pos_control->update_z_controller(); } -void Copter::auto_payload_place_run_descend() +void Copter::FlightMode_AUTO::payload_place_run_descend() { - land_run_horizontal_control(); - land_run_vertical_control(); + _copter.land_run_horizontal_control(); + _copter.land_run_vertical_control(); } diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index eb2f14e9a7..7a48a8c768 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -61,7 +61,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) break; case AUTO: - success = auto_init(ignore_checks); + success = flightmode_auto.init(ignore_checks); + if (success) { + flightmode = &flightmode_auto; + } break; case CIRCLE: @@ -193,10 +196,6 @@ void Copter::update_flight_mode() } switch (control_mode) { - case AUTO: - auto_run(); - break; - case CIRCLE: circle_run(); break; @@ -327,7 +326,6 @@ bool Copter::mode_requires_GPS() return flightmode->requires_GPS(); } switch (control_mode) { - case AUTO: case GUIDED: case LOITER: case RTL: @@ -381,7 +379,6 @@ void Copter::notify_flight_mode() return; } switch (control_mode) { - case AUTO: case GUIDED: case RTL: case CIRCLE: @@ -400,9 +397,6 @@ void Copter::notify_flight_mode() // set flight mode string switch (control_mode) { - case AUTO: - notify.set_flight_mode_str("AUTO"); - break; case GUIDED: notify.set_flight_mode_str("GUID"); break; diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 1c32f9f3b2..bbaeff9e5d 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -23,8 +23,8 @@ void Copter::heli_init() // should be called at 50hz void Copter::check_dynamic_flight(void) { - if (!motors->armed() || !motors->rotor_runup_complete() || - control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && auto_mode == Auto_Land)) { + if (!motor->armed() || !motors->rotor_runup_complete() || + control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && flightmode_auto.mode() == Auto_Land)) { heli_dynamic_flight_counter = 0; heli_flags.dynamic_flight = false; return; @@ -117,7 +117,7 @@ void Copter::heli_update_landing_swash() break; case AUTO: - if (auto_mode == Auto_Land) { + if (flightmode_auto.mode() == Auto_Land) { motors->set_collective_for_landing(true); }else{ motors->set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed); diff --git a/ArduCopter/landing_gear.cpp b/ArduCopter/landing_gear.cpp index 47a44b07bb..86b0c0a525 100644 --- a/ArduCopter/landing_gear.cpp +++ b/ArduCopter/landing_gear.cpp @@ -16,8 +16,7 @@ void Copter::landinggear_update() // To-Do: should we pause the auto-land procedure to give time for gear to come down? if (control_mode == LAND || (control_mode == RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) || - (control_mode == AUTO && auto_mode == Auto_Land) || - (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) { + (control_mode == AUTO && flightmode_auto.landing_gear_should_be_deployed())) { landinggear.set_position(AP_LandingGear::LandingGear_Deploy_And_Keep_Deployed); }