Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: auto mode starts or updates mission from run method

Browse Source
This commit is contained in:
Randy Mackay 2021-03-04 14:04:02 +09:00
parent 06a7086562
commit 1c350514ca
2 changed files with 17 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/mode.h
View File

@ -394,7 +394,6 @@ protected:
int32_t wp_bearing() const override; int32_t wp_bearing() const override;
float crosstrack_error() const override { return wp_nav->crosstrack_error();} float crosstrack_error() const override { return wp_nav->crosstrack_error();}
bool get_wp(Location &loc) override; bool get_wp(Location &loc) override;
void run_autopilot() override;
private: private:
@ -517,7 +516,7 @@ private:
float descend_max; // centimetres float descend_max; // centimetres
} nav_payload_place; } nav_payload_place;
bool waiting_for_origin; // true if waiting for origin before starting mission bool waiting_to_start; // true if waiting for vehicle to be armed or EKF origin before starting mission
}; };
#if AUTOTUNE_ENABLED == ENABLED #if AUTOTUNE_ENABLED == ENABLED

42
ArduCopter/mode_auto.cpp
View File

@ -40,19 +40,12 @@ bool ModeAuto::init(bool ignore_checks)
// initialise waypoint and spline controller // initialise waypoint and spline controller
wp_nav->wp_and_spline_init(); wp_nav->wp_and_spline_init();
// set flag to start mission
waiting_to_start = true;
// clear guided limits // clear guided limits
copter.mode_guided.limit_clear(); copter.mode_guided.limit_clear();
// don't start the mission until we have an origin
Location loc;
if (copter.ahrs.get_origin(loc)) {
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
waiting_for_origin = false;
} else {
waiting_for_origin = true;
}
return true; return true;
} else { } else {
return false; return false;
@ -61,9 +54,21 @@ bool ModeAuto::init(bool ignore_checks)
// auto_run - runs the auto controller // auto_run - runs the auto controller
// should be called at 100hz or more // should be called at 100hz or more
// relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands
void ModeAuto::run() void ModeAuto::run()
{ {
// start or update mission
if (waiting_to_start) {
// don't start the mission until we have an origin
Location loc;
if (copter.ahrs.get_origin(loc)) {
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
waiting_to_start = false;
}
} else {
mission.update();
}
// call the correct auto controller // call the correct auto controller
switch (_mode) { switch (_mode) {
@ -614,21 +619,6 @@ bool ModeAuto::get_wp(Location& destination)
} }
} }
// update mission
void ModeAuto::run_autopilot()
{
Location loc;
if (waiting_for_origin) {
if (copter.ahrs.get_origin(loc)) {
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
waiting_for_origin = false;
}
} else {
mission.update();
}
}
/******************************************************************************* /*******************************************************************************
Verify command Handlers Verify command Handlers