aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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;
float crosstrack_error() const override { return wp_nav->crosstrack_error();}
bool get_wp(Location &loc) override;
void run_autopilot() override;
private:
@ -517,7 +516,7 @@ private:
float descend_max; // centimetres
} 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

42
ArduCopter/mode_auto.cpp
View File

@ -40,19 +40,12 @@ bool ModeAuto::init(bool ignore_checks)
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
// set flag to start mission
waiting_to_start = true;
// clear guided limits
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;
} else {
return false;
@ -61,9 +54,21 @@ bool ModeAuto::init(bool ignore_checks)
// auto_run - runs the auto controller
// 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()
{
// 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
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