Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: FlightMode - convert SMARTRTL flight mode

This commit is contained in:
Peter Barker 2017-09-11 09:46:51 +10:00 committed by Randy Mackay
parent b0e34bd307
commit 9c60c1de58
6 changed files with 77 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/ArduCopter.cpp
View File

@ -535,6 +535,11 @@ void Copter::update_GPS(void)
}
}
void Copter::smart_rtl_save_position()
{
flightmode_smartrtl.save_position();
}
void Copter::init_simple_bearing()
{
// capture current cos_yaw and sin_yaw values

1
ArduCopter/Copter.cpp
View File

@ -33,7 +33,6 @@ Copter::Copter(void)
home_bearing(0),
home_distance(0),
wp_distance(0),
smart_rtl_state(SmartRTL_PathFollow),
simple_cos_yaw(1.0f),
simple_sin_yaw(0.0f),
super_simple_last_bearing(0),

14
ArduCopter/Copter.h
View File

@ -392,9 +392,6 @@ private:
float descend_max; // centimetres
} nav_payload_place;
// SmartRTL
SmartRTLState smart_rtl_state; // records state of SmartRTL
// SIMPLE Mode
// Used to track the orientation of the vehicle for Simple mode. This value is reset at each arming
// or in SuperSimple mode when the vehicle leaves a 20m radius from home.
@ -770,14 +767,7 @@ private:
void set_mode_land_with_pause(mode_reason_t reason);
bool landing_with_GPS();
bool smart_rtl_init(bool ignore_checks);
void smart_rtl_exit();
void smart_rtl_run();
void smart_rtl_wait_cleanup_run();
void smart_rtl_path_follow_run();
void smart_rtl_pre_land_position_run();
void smart_rtl_land();
void smart_rtl_save_position();
void smart_rtl_save_position(); // method for scheduler to call
void crash_check();
void parachute_check();
@ -1020,6 +1010,8 @@ private:
Copter::FlightMode_GUIDED_NOGPS flightmode_guided_nogps{*this};
Copter::FlightMode_SMARTRTL flightmode_smartrtl{*this};
public:
void mavlink_delay_cb();
void failsafe_check();

48
ArduCopter/FlightMode.h
View File

@ -544,6 +544,13 @@ protected:
const char *name() const override { return "RTL"; }
const char *name4() const override { return "RTL "; }
void descent_start();
void descent_run();
void land_start();
void land_run(bool disarm_on_land);
void set_descent_target_alt(uint32_t alt) { rtl_path.descent_target.alt = alt; }
private:
void climb_start();
@ -551,10 +558,6 @@ private:
void climb_return_run();
void loiterathome_start();
void loiterathome_run();
void descent_start();
void descent_run();
void land_start();
void land_run(bool disarm_on_land);
void build_path(bool terrain_following_allowed);
void compute_return_target(bool terrain_following_allowed);
@ -895,3 +898,40 @@ protected:
private:
};
class FlightMode_SMARTRTL : public FlightMode_RTL {
public:
FlightMode_SMARTRTL(Copter &copter) :
FlightMode_SMARTRTL::FlightMode_RTL(copter)
{ }
bool init(bool ignore_checks) override;
void run() override; // should be called at 100hz or more
bool requires_GPS() const override { return true; }
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; }
void save_position();
void exit();
protected:
const char *name() const override { return "SMARTRTL"; }
const char *name4() const override { return "SRTL"; }
private:
void wait_cleanup_run();
void path_follow_run();
void pre_land_position_run();
void land();
SmartRTLState smart_rtl_state = SmartRTL_PathFollow;
};

40
ArduCopter/control_smart_rtl.cpp
View File

@ -7,9 +7,9 @@
* Once the copter is close to home, it will run a standard land controller.
*/
bool Copter::smart_rtl_init(bool ignore_checks)
bool Copter::FlightMode_SMARTRTL::init(bool ignore_checks)
{
if ((position_ok() || ignore_checks) && g2.smart_rtl.is_active()) {
if ((copter.position_ok() || ignore_checks) && g2.smart_rtl.is_active()) {
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
@ -20,7 +20,7 @@ bool Copter::smart_rtl_init(bool ignore_checks)
wp_nav->set_wp_destination(stopping_point);
// initialise yaw to obey user parameter
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
copter.set_auto_yaw_mode(copter.get_default_auto_yaw_mode(true));
// wait for cleanup of return path
smart_rtl_state = SmartRTL_WaitForPathCleanup;
@ -31,33 +31,33 @@ bool Copter::smart_rtl_init(bool ignore_checks)
}
// perform cleanup required when leaving smart_rtl
void Copter::smart_rtl_exit()
void Copter::FlightMode_SMARTRTL::exit()
{
g2.smart_rtl.cancel_request_for_thorough_cleanup();
}
void Copter::smart_rtl_run()
void Copter::FlightMode_SMARTRTL::run()
{
switch (smart_rtl_state) {
case SmartRTL_WaitForPathCleanup:
smart_rtl_wait_cleanup_run();
wait_cleanup_run();
break;
case SmartRTL_PathFollow:
smart_rtl_path_follow_run();
path_follow_run();
break;
case SmartRTL_PreLandPosition:
smart_rtl_pre_land_position_run();
pre_land_position_run();
break;
case SmartRTL_Descend:
rtl_descent_run(); // Re-using the descend method from normal rtl mode.
descent_run(); // Re-using the descend method from normal rtl mode.
break;
case SmartRTL_Land:
rtl_land_run(true); // Re-using the land method from normal rtl mode.
land_run(true); // Re-using the land method from normal rtl mode.
break;
}
}
void Copter::smart_rtl_wait_cleanup_run()
void Copter::FlightMode_SMARTRTL::wait_cleanup_run()
{
// hover at current target position
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@ -71,7 +71,7 @@ void Copter::smart_rtl_wait_cleanup_run()
}
}
void Copter::smart_rtl_path_follow_run()
void Copter::FlightMode_SMARTRTL::path_follow_run()
{
// if we are close to current target point, switch the next point to be our target.
if (wp_nav->reached_wp_destination()) {
@ -108,17 +108,17 @@ void Copter::smart_rtl_path_follow_run()
}
}
void Copter::smart_rtl_pre_land_position_run()
void Copter::FlightMode_SMARTRTL::pre_land_position_run()
{
// if we are close to 2m above start point, we are ready to land.
if (wp_nav->reached_wp_destination()) {
// choose descend and hold, or land based on user parameter rtl_alt_final
if (g.rtl_alt_final <= 0 || failsafe.radio) {
rtl_land_start();
if (g.rtl_alt_final <= 0 || copter.failsafe.radio) {
land_start();
smart_rtl_state = SmartRTL_Land;
} else {
rtl_path.descent_target.alt = g.rtl_alt_final;
rtl_descent_start();
set_descent_target_alt(copter.g.rtl_alt_final);
descent_start();
smart_rtl_state = SmartRTL_Descend;
}
}
@ -131,9 +131,9 @@ void Copter::smart_rtl_pre_land_position_run()
}
// save current position for use by the smart_rtl flight mode
void Copter::smart_rtl_save_position()
void Copter::FlightMode_SMARTRTL::save_position()
{
const bool save_position = motors->armed() && (control_mode != SMART_RTL);
const bool should_save_position = motors->armed() && (copter.control_mode != SMART_RTL);
g2.smart_rtl.update(position_ok(), save_position);
copter.g2.smart_rtl.update(copter.position_ok(), should_save_position);
}

16
ArduCopter/flight_mode.cpp
View File

@ -170,7 +170,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
break;
case SMART_RTL:
success = smart_rtl_init(ignore_checks);
success = flightmode_guided_nogps.init(ignore_checks);
if (success) {
flightmode = &flightmode_smartrtl;
}
break;
default:
@ -239,10 +242,6 @@ void Copter::update_flight_mode()
switch (control_mode) {
case SMART_RTL:
smart_rtl_run();
break;
default:
break;
}
@ -278,7 +277,7 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
// call smart_rtl cleanup
if (old_control_mode == SMART_RTL) {
smart_rtl_exit();
flightmode_smartrtl.exit();
}
#if FRAME_CONFIG == HELI_FRAME
@ -308,8 +307,6 @@ bool Copter::mode_requires_GPS()
return flightmode->requires_GPS();
}
switch (control_mode) {
case SMART_RTL:
return true;
default:
return false;
}
@ -352,9 +349,6 @@ void Copter::notify_flight_mode()
return;
}
switch (control_mode) {
case SMART_RTL:
// autopilot modes
AP_Notify::flags.autopilot_mode = true;
break;
default:
// all other are manual flight modes