aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: Remove land/landed code

Browse Source
This commit is contained in:
Jacob Walser 2016-11-30 11:46:08 -05:00 committed by Andrew Tridgell
parent b6d76912cf
commit a430f5a7ae
14 changed files with 30 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduSub/ArduSub.cpp
View File

@ -274,9 +274,6 @@ void Sub::fast_loop()
// update home from EKF if necessary
update_home_from_EKF();
// check if we've landed or crashed
update_land_and_crash_detectors();
// check if we've reached the surface or bottom
update_surface_and_bottom_detector();
@ -302,9 +299,6 @@ void Sub::rc_loop()
// ---------------------------
void Sub::throttle_loop()
{
// update throttle_low_comp value (controls priority of throttle vs attitude control)
update_throttle_thr_mix();
// check auto_armed status
update_auto_armed();

2
ArduSub/GCS_Mavlink.cpp
View File

@ -29,7 +29,7 @@ void Sub::gcs_send_deferred(void)
NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = ap.land_complete ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE;
uint8_t system_status = motors.armed() ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
// set system as critical if any failsafe have triggered

8
ArduSub/Sub.h
View File

@ -236,14 +236,12 @@ private:
uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised
uint8_t logging_started : 1; // 6 // true if dataflash logging has started
uint8_t land_complete : 1; // 7 // true if we have detected a landing
uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio
uint8_t usb_connected : 1; // 9 // true if APM is powered from USB connection
uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update
uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration
uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test
uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes
uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete)
uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
uint8_t system_time_set : 1; // 16 // true if the system time has been set from the GPS
uint8_t gps_base_pos_set : 1; // 17 // true when the gps base position has been set (used for RTK gps only)
@ -568,8 +566,6 @@ private:
void set_simple_mode(uint8_t b);
void set_failsafe_radio(bool b);
void set_failsafe_battery(bool b);
void set_land_complete(bool b);
void set_land_complete_maybe(bool b);
void set_pre_arm_check(bool b);
void set_pre_arm_rc_check(bool b);
void update_using_interlock();
@ -804,10 +800,6 @@ private:
void check_dynamic_flight(void);
void read_inertia();
void read_inertial_altitude();
bool land_complete_maybe();
void update_land_and_crash_detectors();
void update_land_detector();
void update_throttle_thr_mix();
void update_surface_and_bottom_detector();
void set_surfaced(bool at_surface);
void set_bottomed(bool at_bottom);

39
ArduSub/commands_logic.cpp
View File

@ -186,9 +186,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp(cmd);
case MAV_CMD_NAV_LAND:
return verify_land();
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited();
@ -592,42 +589,6 @@ void Sub::do_guided_limits(const AP_Mission::Mission_Command& cmd)
// Verify Nav (Must) commands
/********************************************************************************/
// verify_land - returns true if landing has been completed
bool Sub::verify_land()
{
bool retval = false;
switch( land_state ) {
case LAND_STATE_FLY_TO_LOCATION:
// check if we've reached the location
if (wp_nav.reached_wp_destination()) {
// get destination so we can use it for loiter target
Vector3f dest = wp_nav.get_wp_destination();
// initialise landing controller
auto_land_start(dest);
// advance to next state
land_state = LAND_STATE_DESCENDING;
}
break;
case LAND_STATE_DESCENDING:
// rely on THROTTLE_LAND mode to correctly update landing status
retval = ap.land_complete;
break;
default:
// this should never happen
// TO-DO: log an error
retval = true;
break;
}
// true is returned if we've successfully landed
return retval;
}
// verify_nav_wp - check if we have reached the next way point
bool Sub::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{

7
ArduSub/compassmot.cpp
View File

@ -69,13 +69,6 @@ uint8_t Sub::mavlink_compassmot(mavlink_channel_t chan)
return 1;
}
// check we are landed
if (!ap.land_complete) {
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Not landed");
ap.compass_mot = false;
return 1;
}
// disable cpu failsafe
failsafe_disable();

14
ArduSub/control_circle.cpp
View File

@ -42,7 +42,7 @@ void Sub::circle_run()
pos_control.set_accel_z(g.pilot_accel_z);
// 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()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
// To-Do: add some initialisation of position controllers
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
@ -63,11 +63,13 @@ void Sub::circle_run()
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
}
// // check for pilot requested take-off
// 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();
// }
}
// set motors to full range

8
ArduSub/control_guided.cpp
View File

@ -346,7 +346,7 @@ void Sub::guided_takeoff_run()
void Sub::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) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -388,7 +388,7 @@ void Sub::guided_pos_control_run()
void Sub::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) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
// initialise velocity controller
pos_control.init_vel_controller_xyz();
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -435,7 +435,7 @@ void Sub::guided_vel_control_run()
void Sub::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) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
// set target position and velocity to current position and velocity
pos_control.set_pos_target(inertial_nav.get_position());
pos_control.set_desired_velocity(Vector3f(0,0,0));
@ -504,7 +504,7 @@ void Sub::guided_posvel_control_run()
void Sub::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) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);

4
ArduSub/control_manual.cpp
View File

@ -4,10 +4,6 @@
// manual_init - initialise manual controller
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)) {
return false;
}
// set target altitude to zero for reporting
pos_control.set_alt_target(0);

18
ArduSub/control_rtl.cpp
View File

@ -348,7 +348,7 @@ void Sub::rtl_land_run()
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
@ -358,18 +358,18 @@ void Sub::rtl_land_run()
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
init_disarm_motors();
}
// if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
// init_disarm_motors();
// }
#else
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
}
// if (ap.land_complete) {
// init_disarm_motors();
// }
#endif
// check if we've completed this stage of RTL
rtl_state_complete = ap.land_complete;
// rtl_state_complete = ap.land_complete;
return;
}
@ -414,7 +414,7 @@ void Sub::rtl_land_run()
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// check if we've completed this stage of RTL
rtl_state_complete = ap.land_complete;
// rtl_state_complete = ap.land_complete;
}
void Sub::rtl_build_path(bool terrain_following_allowed)

4
ArduSub/defines.h
View File

@ -313,8 +313,8 @@ enum ThrowModeState {
#define DATA_ARMED 10
#define DATA_DISARMED 11
#define DATA_AUTO_ARMED 15
#define DATA_LAND_COMPLETE_MAYBE 17
#define DATA_LAND_COMPLETE 18
//#define DATA_LAND_COMPLETE_MAYBE 17
//#define DATA_LAND_COMPLETE 18
#define DATA_NOT_LANDED 28
#define DATA_LOST_GPS 19
//#define DATA_FLIP_START 21

7
ArduSub/events.cpp
View File

@ -298,16 +298,17 @@ bool Sub::should_disarm_on_failsafe() {
case STABILIZE:
case ACRO:
// if throttle is zero OR vehicle is landed disarm motors
return ap.throttle_zero || ap.land_complete;
return ap.throttle_zero;
break;
case AUTO:
// if mission has not started AND vehicle is landed, disarm motors
return !ap.auto_armed && ap.land_complete;
return !ap.auto_armed;
break;
default:
// used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
// if landed disarm
return ap.land_complete;
// return ap.land_complete;
return false;
break;
}
}

2
ArduSub/flight_mode.cpp
View File

@ -225,7 +225,7 @@ void Sub::exit_mode(control_mode_t old_control_mode, control_mode_t new_control_
}
// smooth throttle transition when switching from manual to automatic flight modes
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed()) {
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->control_in));
}

69
ArduSub/land_detector.cpp
View File

@ -1,69 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Sub.h"
#define LAND_END_DEPTH -20 //Landed at 20cm depth
// Code to detect a crash main ArduCopter code
#define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing
#define LAND_CHECK_LARGE_ANGLE_CD 1500.0f // maximum angle target to be considered landing
#define LAND_CHECK_ACCEL_MOVING 3.0f // maximum acceleration after subtracting gravity
// counter to verify landings
static uint32_t land_detector_count = 0;
// run land and crash detectors
// called at MAIN_LOOP_RATE
void Sub::update_land_and_crash_detectors()
{
// update 1hz filtered acceleration
Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS;
land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS);
update_land_detector();
crash_check();
}
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at MAIN_LOOP_RATE
void Sub::update_land_detector()
{
// if(barometer.num_instances() > 1 && barometer.get_altitude() > LAND_END_DEPTH && ap.throttle_zero) {
// set_land_complete(true);
// } else {
set_land_complete(false);
// }
}
void Sub::set_land_complete(bool b)
{
// if no change, exit immediately
if( ap.land_complete == b )
return;
land_detector_count = 0;
if(b){
Log_Write_Event(DATA_LAND_COMPLETE);
} else {
Log_Write_Event(DATA_NOT_LANDED);
}
ap.land_complete = b;
// trigger disarm-on-land if configured
bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0;
bool mode_disarms_on_land = mode_allows_arming(control_mode,false) && !mode_has_manual_throttle(control_mode);
if (ap.land_complete && motors.armed() && disarm_on_land_configured && mode_disarms_on_land) {
init_disarm_motors();
}
}
// update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state
// low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
// has no effect when throttle is above hover throttle
void Sub::update_throttle_thr_mix()
{
return; // placeholder, was used by heli only
}

6
ArduSub/motors.cpp
View File

@ -114,7 +114,7 @@ void Sub::auto_disarm_check()
thr_low = g.rc_3.control_in <= deadband_top;
}
if (!thr_low || !ap.land_complete) {
if (!thr_low) {
// reset timer
auto_disarm_begin = tnow_ms;
}
@ -246,10 +246,6 @@ void Sub::init_disarm_motors()
autotune_save_tuning_gains();
#endif
// we are not in the air
// set_land_complete(true);// We will let the land detector decide this in sub
// set_land_complete_maybe(true);
// log disarm to the dataflash
Log_Write_Event(DATA_DISARMED);