Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Remove takeoff code

Browse Source
This commit is contained in:
Jacob Walser 2016-11-30 19:28:46 -05:00 committed by Andrew Tridgell
parent 41d892666b
commit b6d76912cf
12 changed files with 3 additions and 263 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduSub/GCS_Mavlink.cpp
View File

@ -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;

17
ArduSub/Parameters.cpp
View File

@ -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.

2
ArduSub/Parameters.h
View File

@ -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;

4
ArduSub/Sub.cpp
View File

@ -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),

26
ArduSub/Sub.h
View File

@ -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);

21
ArduSub/commands_logic.cpp
View File

@ -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()
{

7
ArduSub/config.h
View File

@ -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

2
ArduSub/control_circle.cpp
View File

@ -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();
}
}

2
ArduSub/control_manual.cpp
View File

@ -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

2
ArduSub/flight_mode.cpp
View File

@ -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

18
ArduSub/switches.cpp
View File

@ -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;

147
ArduSub/takeoff.cpp
View File

@ -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());
}