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

Rover: mission started only after EKF origin is set

This commit is contained in:
Randy Mackay 2021-11-15 15:06:45 +09:00 committed by Andrew Tridgell
parent 7618241be4
commit 1f28ab0576
4 changed files with 17 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Rover/Rover.cpp
View File

@ -85,7 +85,6 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
SCHED_TASK_CLASS(AC_Fence, &rover.g2.fence, update, 10, 100, 33),
SCHED_TASK(update_wheel_encoder, 50, 200, 36),
SCHED_TASK(update_compass, 10, 200, 39),
SCHED_TASK(update_mission, 50, 200, 42),
SCHED_TASK(update_logging1, 10, 200, 45),
SCHED_TASK(update_logging2, 10, 200, 48),
SCHED_TASK_CLASS(GCS, (GCS*)&rover._gcs, update_receive, 400, 500, 51),
@ -408,16 +407,6 @@ void Rover::update_current_mode(void)
control_mode->update();
}
// update mission including starting or stopping commands. called by scheduler at 10Hz
void Rover::update_mission(void)
{
if (control_mode == &mode_auto) {
if (ahrs.home_is_set() && mode_auto.mission.num_commands() > 1) {
mode_auto.mission.update();
}
}
}
// vehicle specific waypoint info helpers
bool Rover::get_wp_distance_m(float &distance) const
{

1
Rover/Rover.h
View File

@ -279,7 +279,6 @@ private:
void update_logging2(void);
void one_second_loop(void);
void update_current_mode(void);
void update_mission(void);
// balance_bot.cpp
void balancebot_pitch_control(float &throttle);

1
Rover/mode.h
View File

@ -331,6 +331,7 @@ private:
MIS_DONE_BEHAVE_MANUAL = 3
};
bool waiting_to_start; // true if waiting for EKF origin before starting mission
bool auto_triggered; // true when auto has been triggered to start
// HeadingAndSpeed sub mode variables

18
Rover/mode_auto.cpp
View File

@ -29,8 +29,9 @@ bool ModeAuto::_enter()
start_stop();
}
// restart mission processing
mission.start_or_resume();
// set flag to start mission
waiting_to_start = true;
return true;
}
@ -44,6 +45,19 @@ void ModeAuto::_exit()
void ModeAuto::update()
{
// start or update mission
if (waiting_to_start) {
// don't start the mission until we have an origin
Location loc;
if (ahrs.get_origin(loc)) {
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
waiting_to_start = false;
}
} else {
mission.update();
}
switch (_submode) {
case Auto_WP:
{