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

Copter: FlightMode - convert LAND flight mode

Browse Source
This commit is contained in:
Peter Barker 2016-03-22 14:24:56 +11:00 committed by Randy Mackay
parent 2db09ba0f7
commit 3b1ca99b95
5 changed files with 69 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/Copter.h
View File

@ -840,14 +840,8 @@ private:
void flip_run();
bool guided_nogps_init(bool ignore_checks);
void guided_nogps_run();
bool land_init(bool ignore_checks);
void land_run();
void land_gps_run();
void land_nogps_run();
int32_t land_get_alt_above_ground(void);
void land_run_vertical_control(bool pause_descent = false);
void land_run_horizontal_control();
void land_do_not_use_GPS();
void set_mode_land_with_pause(mode_reason_t reason);
bool landing_with_GPS();
bool poshold_init(bool ignore_checks);
@ -1104,6 +1098,8 @@ private:
Copter::FlightMode_GUIDED flightmode_guided{*this};
Copter::FlightMode_LAND flightmode_land{*this};
Copter::FlightMode_LOITER flightmode_loiter{*this};
#if FRAME_CONFIG == HELI_FRAME

35
ArduCopter/FlightMode.h
View File

@ -475,3 +475,38 @@ private:
GuidedMode guided_mode = Guided_TakeOff;
};
class FlightMode_LAND : public FlightMode {
public:
FlightMode_LAND(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 false; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return false; };
bool is_autopilot() const override { return true; }
float get_land_descent_speed();
bool landing_with_GPS();
void do_not_use_GPS();
int32_t get_alt_above_ground(void);
protected:
const char *name() const override { return "LAND"; }
const char *name4() const override { return "LAND"; }
private:
void gps_run();
void nogps_run();
};

51
ArduCopter/control_land.cpp
View File

@ -1,15 +1,16 @@
#include "Copter.h"
// FIXME? why are these static?
static bool land_with_gps;
static uint32_t land_start_time;
static bool land_pause;
// land_init - initialise land controller
bool Copter::land_init(bool ignore_checks)
bool Copter::FlightMode_LAND::init(bool ignore_checks)
{
// check if we have GPS and decide which LAND we're going to do
land_with_gps = position_ok();
land_with_gps = _copter.position_ok();
if (land_with_gps) {
// set target to stopping point
Vector3f stopping_point;
@ -39,19 +40,19 @@ bool Copter::land_init(bool ignore_checks)
// land_run - runs the land controller
// should be called at 100hz or more
void Copter::land_run()
void Copter::FlightMode_LAND::run()
{
if (land_with_gps) {
land_gps_run();
gps_run();
}else{
land_nogps_run();
nogps_run();
}
}
// land_gps_run - runs the land controller
// horizontal position controlled with loiter controller
// should be called at 100hz or more
void Copter::land_gps_run()
void Copter::FlightMode_LAND::gps_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()) {
@ -68,7 +69,7 @@ void Copter::land_gps_run()
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
_copter.init_disarm_motors();
}
return;
}
@ -81,24 +82,24 @@ void Copter::land_gps_run()
land_pause = false;
}
land_run_horizontal_control();
land_run_vertical_control(land_pause);
_copter.land_run_horizontal_control();
_copter.land_run_vertical_control(land_pause);
}
// land_nogps_run - runs the land controller
// pilot controls roll and pitch angles
// should be called at 100hz or more
void Copter::land_nogps_run()
void Copter::FlightMode_LAND::nogps_run()
{
float target_roll = 0.0f, target_pitch = 0.0f;
float target_yaw_rate = 0;
// process pilot inputs
if (!failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
if (!_copter.failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && _copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
_copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
if (g.land_repositioning) {
@ -106,7 +107,7 @@ void Copter::land_nogps_run()
update_simple_mode();
// get pilot desired lean angles
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max);
}
// get pilot's desired yaw rate
@ -127,7 +128,7 @@ void Copter::land_nogps_run()
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
_copter.init_disarm_motors();
}
return;
}
@ -143,21 +144,21 @@ void Copter::land_nogps_run()
land_pause = false;
}
land_run_vertical_control(land_pause);
_copter.land_run_vertical_control(land_pause);
}
/*
get a height above ground estimate for landing
*/
int32_t Copter::land_get_alt_above_ground(void)
int32_t Copter::FlightMode_LAND::get_alt_above_ground(void)
{
int32_t alt_above_ground;
if (rangefinder_alt_ok()) {
alt_above_ground = rangefinder_state.alt_cm_filt.get();
if (_copter.rangefinder_alt_ok()) {
alt_above_ground = _copter.rangefinder_state.alt_cm_filt.get();
} else {
bool navigating = pos_control->is_active_xy();
if (!navigating || !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
alt_above_ground = current_loc.alt;
if (!navigating || !_copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
alt_above_ground = _copter.current_loc.alt;
}
}
return alt_above_ground;
@ -176,7 +177,7 @@ void Copter::land_run_vertical_control(bool pause_descent)
// compute desired velocity
const float precland_acceptable_error = 15.0f;
const float precland_min_descent_speed = 10.0f;
int32_t alt_above_ground = land_get_alt_above_ground();
int32_t alt_above_ground = flightmode_land.get_alt_above_ground();
float cmb_rate = 0;
if (!pause_descent) {
@ -280,7 +281,7 @@ void Copter::land_run_horizontal_control()
// there is any position estimate drift after touchdown. We
// limit attitude to 7 degrees below this limit and linearly
// interpolate for 1m above that
int alt_above_ground = land_get_alt_above_ground();
int alt_above_ground = flightmode_land.get_alt_above_ground();
float attitude_limit_cd = linear_interpolate(700, aparm.angle_max, alt_above_ground,
g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U);
float total_angle_cd = norm(nav_roll, nav_pitch);
@ -299,10 +300,10 @@ void Copter::land_run_horizontal_control()
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain());
}
// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
// do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
// called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
// has no effect if we are not already in LAND mode
void Copter::land_do_not_use_GPS()
void Copter::FlightMode_LAND::do_not_use_GPS()
{
land_with_gps = false;
}

2
ArduCopter/ekf_check.cpp
View File

@ -175,7 +175,7 @@ void Copter::failsafe_ekf_event()
// if flight mode is already LAND ensure it's not the GPS controlled LAND
if (control_mode == LAND) {
land_do_not_use_GPS();
flightmode_land.do_not_use_GPS();
}
}

15
ArduCopter/flight_mode.cpp
View File

@ -89,7 +89,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
break;
case LAND:
success = land_init(ignore_checks);
success = flightmode_land.init(ignore_checks);
if (success) {
flightmode = &flightmode_land;
}
break;
case RTL:
@ -206,10 +209,6 @@ void Copter::update_flight_mode()
switch (control_mode) {
case LAND:
land_run();
break;
case RTL:
rtl_run();
break;
@ -377,12 +376,11 @@ void Copter::notify_flight_mode()
case RTL:
case AVOID_ADSB:
case GUIDED_NOGPS:
case LAND:
case SMART_RTL:
// autopilot modes
AP_Notify::flags.autopilot_mode = true;
break;
default:
default:
// all other are manual flight modes
AP_Notify::flags.autopilot_mode = false;
break;
@ -393,9 +391,6 @@ void Copter::notify_flight_mode()
case RTL:
notify.set_flight_mode_str("RTL ");
break;
case LAND:
notify.set_flight_mode_str("LAND");
break;
case DRIFT:
notify.set_flight_mode_str("DRIF");
break;