From b6d76912cfedda30c0c63a7b4420c722d8f6ab2d Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Wed, 30 Nov 2016 19:28:46 -0500 Subject: [PATCH] Sub: Remove takeoff code --- ArduSub/GCS_Mavlink.cpp | 18 ----- ArduSub/Parameters.cpp | 17 ----- ArduSub/Parameters.h | 2 - ArduSub/Sub.cpp | 4 +- ArduSub/Sub.h | 26 ------- ArduSub/commands_logic.cpp | 21 ------ ArduSub/config.h | 7 -- ArduSub/control_circle.cpp | 2 - ArduSub/control_manual.cpp | 2 +- ArduSub/flight_mode.cpp | 2 - ArduSub/switches.cpp | 18 ----- ArduSub/takeoff.cpp | 147 ------------------------------------- 12 files changed, 3 insertions(+), 263 deletions(-) delete mode 100644 ArduSub/takeoff.cpp diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 4b69234910..fd7d0bbf9d 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1256,24 +1256,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } break; - case MAV_CMD_NAV_TAKEOFF: { - // param3 : horizontal navigation by pilot acceptable - // param4 : yaw angle (not supported) - // param5 : latitude (not supported) - // param6 : longitude (not supported) - // param7 : altitude [metres] - - float takeoff_alt = packet.param7 * 100; // Convert m to cm - - if(sub.do_user_takeoff(takeoff_alt, is_zero(packet.param3))) { - result = MAV_RESULT_ACCEPTED; - } else { - result = MAV_RESULT_FAILED; - } - break; - } - - case MAV_CMD_NAV_LOITER_UNLIM: if (sub.set_mode(POSHOLD, MODE_REASON_GCS_COMMAND)) { result = MAV_RESULT_ACCEPTED; diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index b9f909abd0..7e0195abfc 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -84,23 +84,6 @@ const AP_Param::Info Sub::var_info[] = { // @Increment: .5 GSCALAR(throttle_filt, "PILOT_THR_FILT", 0), - // @Param: PILOT_TKOFF_ALT - // @DisplayName: Pilot takeoff altitude - // @Description: Altitude that altitude control modes will climb to when a takeoff is triggered with the throttle stick. - // @User: Standard - // @Units: Centimeters - // @Range: 0.0 1000.0 - // @Increment: 10 - GSCALAR(pilot_takeoff_alt, "PILOT_TKOFF_ALT", PILOT_TKOFF_ALT_DEFAULT), - - // @Param: PILOT_TKOFF_DZ - // @DisplayName: Takeoff trigger deadzone - // @Description: Offset from mid stick at which takeoff is triggered - // @User: Standard - // @Range: 0.0 500.0 - // @Increment: 10 - GSCALAR(takeoff_trigger_dz, "PILOT_TKOFF_DZ", THR_DZ_DEFAULT), - // @Param: PILOT_THR_BHV // @DisplayName: Throttle stick behavior // @Description: Bitmask containing various throttle stick options. Add up the values for options that you want. diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 7a9b3c91ef..baf55ba492 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -425,8 +425,6 @@ public: AP_Float throttle_filt; AP_Int16 throttle_behavior; - AP_Int16 takeoff_trigger_dz; - AP_Float pilot_takeoff_alt; AP_Int16 rtl_altitude; AP_Int16 rtl_speed_cms; diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index a684dbb615..35d3be1373 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -34,8 +34,8 @@ Sub::Sub(void) : home_bearing(0), home_distance(0), wp_distance(0), - auto_mode(Auto_TakeOff), - guided_mode(Guided_TakeOff), + auto_mode(Auto_WP), + guided_mode(Guided_WP), rtl_state(RTL_InitialClimb), rtl_state_complete(false), circle_pilot_yaw_override(false), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index f6b59cb018..47f599257a 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -266,18 +266,9 @@ private: control_mode_t prev_control_mode; mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN; - struct { - bool running; - float max_speed; - float alt_delta; - uint32_t start_ms; - } takeoff_state; uint32_t precland_last_update_ms; - // altitude below which we do no navigation in auto takeoff - float auto_takeoff_no_nav_alt_cm; - RCMapper rcmap; // board specific config @@ -590,15 +581,9 @@ private: float get_roi_yaw(); float get_look_ahead_yaw(); void update_throttle_hover(); - void set_throttle_takeoff(); float get_pilot_desired_throttle(int16_t throttle_control); float get_pilot_desired_climb_rate(float throttle_control); - float get_non_takeoff_throttle(); - float get_takeoff_trigger_throttle(); - float get_throttle_pre_takeoff(float input_thr); float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); - void auto_takeoff_set_start_alt(void); - void auto_takeoff_attitude_run(float target_yaw_rate); void set_accel_throttle_I_from_pilot_throttle(float pilot_throttle); void update_poscon_alt_max(); void rotate_body_frame_to_NE(float &x, float &y); @@ -669,7 +654,6 @@ private: void set_system_time_from_GPS(); void exit_mission(); void do_RTL(void); - bool verify_takeoff(); bool verify_land(); bool verify_loiter_unlimited(); bool verify_loiter_time(); @@ -687,8 +671,6 @@ private: void althold_run(); 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(); @@ -735,7 +717,6 @@ private: bool circle_init(bool ignore_checks); void circle_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(); @@ -746,7 +727,6 @@ private: void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); void guided_set_angle(const Quaternion &q, float climb_rate_cms); void guided_run(); - void guided_takeoff_run(); void guided_pos_control_run(); void guided_vel_control_run(); void guided_posvel_control_run(); @@ -934,11 +914,6 @@ private: void update_auto_armed(); void check_usb_mux(void); bool should_log(uint32_t mask); - bool current_mode_has_user_takeoff(bool must_navigate); - bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate); - void takeoff_timer_start(float alt_cm); - void takeoff_stop(); - void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate); void print_hit_enter(); void tuning(); void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...); @@ -947,7 +922,6 @@ private: bool verify_command_callback(const AP_Mission::Mission_Command& cmd); bool do_guided(const AP_Mission::Mission_Command& cmd); - void do_takeoff(const AP_Mission::Mission_Command& cmd); void do_nav_wp(const AP_Mission::Mission_Command& cmd); void do_land(const AP_Mission::Mission_Command& cmd); void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 5e10df039d..698e845e57 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -15,10 +15,6 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd) /// /// navigation commands /// - case MAV_CMD_NAV_TAKEOFF: // 22 - do_takeoff(cmd); - break; - case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint do_nav_wp(cmd); break; @@ -187,9 +183,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd) // // navigation commands // - case MAV_CMD_NAV_TAKEOFF: - return verify_takeoff(); - case MAV_CMD_NAV_WAYPOINT: return verify_nav_wp(cmd); @@ -284,13 +277,6 @@ void Sub::do_RTL(void) // Nav (Must) commands /********************************************************************************/ -// do_takeoff - initiate takeoff navigation command -void Sub::do_takeoff(const AP_Mission::Mission_Command& cmd) -{ - // Set wp navigation target to safe altitude above current position - auto_takeoff_start(cmd.content.location); -} - // do_nav_wp - initiate move to next waypoint void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd) { @@ -606,13 +592,6 @@ void Sub::do_guided_limits(const AP_Mission::Mission_Command& cmd) // Verify Nav (Must) commands /********************************************************************************/ -// verify_takeoff - check if we have completed the takeoff -bool Sub::verify_takeoff() -{ - // have we reached our target altitude? - return wp_nav.reached_wp_destination(); -} - // verify_land - returns true if landing has been completed bool Sub::verify_land() { diff --git a/ArduSub/config.h b/ArduSub/config.h index 7facc07094..73cbba17ff 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -304,13 +304,6 @@ # define FS_THR_VALUE_DEFAULT 975 #endif -////////////////////////////////////////////////////////////////////////////// -// Takeoff -// -#ifndef PILOT_TKOFF_ALT_DEFAULT - # define PILOT_TKOFF_ALT_DEFAULT 0 // default final alt above home for pilot initiated takeoff -#endif - ////////////////////////////////////////////////////////////////////////////// // Landing diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index 6a8c8541c4..abe9c06e23 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -67,8 +67,6 @@ void Sub::circle_run() if (ap.land_complete && target_climb_rate > 0) { // indicate we are taking off set_land_complete(false); - // clear i term when we're taking off - set_throttle_takeoff(); } } diff --git a/ArduSub/control_manual.cpp b/ArduSub/control_manual.cpp index afb3791167..0414c6e8c5 100644 --- a/ArduSub/control_manual.cpp +++ b/ArduSub/control_manual.cpp @@ -5,7 +5,7 @@ bool Sub::manual_init(bool ignore_checks) { // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high - if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) { + if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode)) { return false; } // set target altitude to zero for reporting diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index 2d1b44c452..73c7e773d0 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -230,8 +230,6 @@ void Sub::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_ set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in)); } - // cancel any takeoffs in progress - takeoff_stop(); } // returns true or false whether mode requires GPS diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp index 097835629d..211ec73fa5 100644 --- a/ArduSub/switches.cpp +++ b/ArduSub/switches.cpp @@ -301,24 +301,6 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) // create new mission command AP_Mission::Mission_Command cmd = {}; - // if the mission is empty save a takeoff command - if(mission.num_commands() == 0) { - // set our location ID to 16, MAV_CMD_NAV_WAYPOINT - cmd.id = MAV_CMD_NAV_TAKEOFF; - cmd.content.location.options = 0; - cmd.p1 = 0; - cmd.content.location.lat = 0; - cmd.content.location.lng = 0; - cmd.content.location.alt = MAX(current_loc.alt,100); - - // use the current altitude for the target alt for takeoff. - // only altitude will matter to the AP mission script for takeoff. - if(mission.add_cmd(cmd)) { - // log event - Log_Write_Event(DATA_SAVEWP_ADD_WP); - } - } - // set new waypoint to current location cmd.content.location = current_loc; diff --git a/ArduSub/takeoff.cpp b/ArduSub/takeoff.cpp deleted file mode 100644 index 8b7f038417..0000000000 --- a/ArduSub/takeoff.cpp +++ /dev/null @@ -1,147 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "Sub.h" - -// This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes. -// The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude -// A safe takeoff speed is calculated and used to calculate a time_ms -// the pos_control target is then slowly increased until time_ms expires - -// return true if this flight mode supports user takeoff -// must_nagivate is true if mode must also control horizontal position -bool Sub::current_mode_has_user_takeoff(bool must_navigate) -{ - return false; // not supported in Sub -} - -// initiate user takeoff - called when MAVLink TAKEOFF command is received -bool Sub::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) -{ - return false; // not supported in Sub -} - -// start takeoff to specified altitude above home in centimeters -void Sub::takeoff_timer_start(float alt_cm) -{ - // calculate climb rate - float speed = MIN(wp_nav.get_speed_up(), MAX(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f)); - - // sanity check speed and target - if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) { - return; - } - - // initialise takeoff state - takeoff_state.running = true; - takeoff_state.max_speed = speed; - takeoff_state.start_ms = millis(); - takeoff_state.alt_delta = alt_cm; -} - -// stop takeoff -void Sub::takeoff_stop() -{ - takeoff_state.running = false; - takeoff_state.start_ms = 0; -} - -// returns pilot and takeoff climb rates -// pilot_climb_rate is both an input and an output -// takeoff_climb_rate is only an output -// has side-effect of turning takeoff off when timeout as expired -void Sub::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate) -{ - // return pilot_climb_rate if take-off inactive - if (!takeoff_state.running) { - takeoff_climb_rate = 0.0f; - return; - } - - // acceleration of 50cm/s/s - static const float takeoff_accel = 50.0f; - float takeoff_minspeed = MIN(50.0f,takeoff_state.max_speed); - float time_elapsed = (millis()-takeoff_state.start_ms)*1.0e-3f; - float speed = MIN(time_elapsed*takeoff_accel+takeoff_minspeed, takeoff_state.max_speed); - - float time_to_max_speed = (takeoff_state.max_speed-takeoff_minspeed)/takeoff_accel; - float height_gained; - if (time_elapsed <= time_to_max_speed) { - height_gained = 0.5f*takeoff_accel*sq(time_elapsed) + takeoff_minspeed*time_elapsed; - } else { - height_gained = 0.5f*takeoff_accel*sq(time_to_max_speed) + takeoff_minspeed*time_to_max_speed + - (time_elapsed-time_to_max_speed)*takeoff_state.max_speed; - } - - // check if the takeoff is over - if (height_gained >= takeoff_state.alt_delta) { - takeoff_stop(); - } - - // if takeoff climb rate is zero return - if (speed <= 0.0f) { - takeoff_climb_rate = 0.0f; - return; - } - - // default take-off climb rate to maximum speed - takeoff_climb_rate = speed; - - // if pilot's commands descent - if (pilot_climb_rate < 0.0f) { - // if overall climb rate is still positive, move to take-off climb rate - if (takeoff_climb_rate + pilot_climb_rate > 0.0f) { - takeoff_climb_rate = takeoff_climb_rate + pilot_climb_rate; - pilot_climb_rate = 0; - } else { - // if overall is negative, move to pilot climb rate - pilot_climb_rate = pilot_climb_rate + takeoff_climb_rate; - takeoff_climb_rate = 0.0f; - } - } else { // pilot commands climb - // pilot climb rate is zero until it surpasses the take-off climb rate - if (pilot_climb_rate > takeoff_climb_rate) { - pilot_climb_rate = pilot_climb_rate - takeoff_climb_rate; - } else { - pilot_climb_rate = 0.0f; - } - } -} - -void Sub::auto_takeoff_set_start_alt(void) -{ - // start with our current altitude - auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude(); - - if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) { - // we are not flying, add the takeoff_nav_alt - auto_takeoff_no_nav_alt_cm += g2.wp_navalt_min * 100; - } -} - - -/* - call attitude controller for automatic takeoff, limiting roll/pitch - if below takeoff_nav_alt - */ -void Sub::auto_takeoff_attitude_run(float target_yaw_rate) -{ - float nav_roll, nav_pitch; - - if (g2.wp_navalt_min > 0 && inertial_nav.get_altitude() < auto_takeoff_no_nav_alt_cm) { - // we haven't reached the takeoff navigation altitude yet - nav_roll = 0; - nav_pitch = 0; -#if FRAME_CONFIG == HELI_FRAME - // prevent hover roll starting till past specified altitude - hover_roll_trim_scalar_slew = 0; -#endif - // tell the position controller that we have limited roll/pitch demand to prevent integrator buildup - pos_control.set_limit_accel_xy(); - } else { - nav_roll = wp_nav.get_roll(); - nav_pitch = wp_nav.get_pitch(); - } - - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain()); -}