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

Rover: re-order auto mode implementations

This commit is contained in:
Randy Mackay 2019-03-16 09:43:49 +09:00
parent 7498972b8d
commit 5286b6fab6
2 changed files with 60 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
APMrover2/mode.h
View File

@ -295,11 +295,9 @@ protected:
private:
bool check_trigger(void);
// this is set to true when auto has been triggered to start
bool auto_triggered;
bool _reached_heading; // true when vehicle has reached desired heading in TurnToHeading sub mode
bool start_loiter();
void start_guided(const Location& target_loc);
void send_guided_position_target();
bool start_command(const AP_Mission::Mission_Command& cmd);
void exit_mission();
@ -325,16 +323,15 @@ private:
void do_set_reverse(const AP_Mission::Mission_Command& cmd);
void do_guided_limits(const AP_Mission::Mission_Command& cmd);
bool start_loiter();
void start_guided(const Location& target_loc);
void send_guided_position_target();
enum Mis_Done_Behave {
MIS_DONE_BEHAVE_HOLD = 0,
MIS_DONE_BEHAVE_LOITER = 1,
MIS_DONE_BEHAVE_ACRO = 2
};
bool auto_triggered; // true when auto has been triggered to start
bool _reached_heading; // true when vehicle has reached desired heading in TurnToHeading sub mode
// Loiter control
uint16_t loiter_duration; // How long we should loiter at the nav_waypoint (time in seconds)
uint32_t loiter_start_time; // How long have we been loitering - The start time in millis

110
APMrover2/mode_auto.cpp
View File

@ -105,6 +105,16 @@ void ModeAuto::update()
}
}
void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)
{
// If not autostarting set the throttle to minimum
if (!check_trigger()) {
stop_vehicle();
return;
}
Mode::calc_throttle(target_speed, nudge_allowed, avoidance_enabled);
}
// return distance (in meters) to destination
float ModeAuto::get_distance_to_destination() const
{
@ -177,52 +187,7 @@ void ModeAuto::start_RTL()
}
}
// hand over control to external navigation controller in AUTO mode
void ModeAuto::start_guided(const Location& loc)
{
if (rover.mode_guided.enter()) {
_submode = Auto_Guided;
// initialise guided start time and position as reference for limit checking
rover.mode_guided.limit_init_time_and_location();
// sanity check target location
if ((loc.lat != 0) || (loc.lng != 0)) {
guided_target.loc = loc;
location_sanitize(rover.current_loc, guided_target.loc);
guided_target.valid = true;
} else {
guided_target.valid = false;
}
}
}
// send latest position target to offboard navigation system
void ModeAuto::send_guided_position_target()
{
if (!guided_target.valid) {
return;
}
// send at maximum of 1hz
const uint32_t now_ms = AP_HAL::millis();
if ((guided_target.last_sent_ms == 0) || (now_ms - guided_target.last_sent_ms > AUTO_GUIDED_SEND_TARGET_MS)) {
guided_target.last_sent_ms = now_ms;
// get system id and component id of offboard navigation system
uint8_t sysid;
uint8_t compid;
mavlink_channel_t chan;
if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_ONBOARD_CONTROLLER, sysid, compid, chan)) {
gcs().chan(chan-MAVLINK_COMM_0).send_set_position_target_global_int(sysid, compid, guided_target.loc);
}
}
}
/*
check for triggering of start of auto mode
*/
// check for triggering of start of auto mode
bool ModeAuto::check_trigger(void)
{
// check for user pressing the auto trigger to off
@ -265,16 +230,6 @@ bool ModeAuto::check_trigger(void)
return false;
}
void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)
{
// If not autostarting set the throttle to minimum
if (!check_trigger()) {
stop_vehicle();
return;
}
Mode::calc_throttle(target_speed, nudge_allowed, avoidance_enabled);
}
bool ModeAuto::start_loiter()
{
if (rover.mode_loiter.enter()) {
@ -283,3 +238,46 @@ bool ModeAuto::start_loiter()
}
return false;
}
// hand over control to external navigation controller in AUTO mode
void ModeAuto::start_guided(const Location& loc)
{
if (rover.mode_guided.enter()) {
_submode = Auto_Guided;
// initialise guided start time and position as reference for limit checking
rover.mode_guided.limit_init_time_and_location();
// sanity check target location
if ((loc.lat != 0) || (loc.lng != 0)) {
guided_target.loc = loc;
location_sanitize(rover.current_loc, guided_target.loc);
guided_target.valid = true;
} else {
guided_target.valid = false;
}
}
}
// send latest position target to offboard navigation system
void ModeAuto::send_guided_position_target()
{
if (!guided_target.valid) {
return;
}
// send at maximum of 1hz
const uint32_t now_ms = AP_HAL::millis();
if ((guided_target.last_sent_ms == 0) || (now_ms - guided_target.last_sent_ms > AUTO_GUIDED_SEND_TARGET_MS)) {
guided_target.last_sent_ms = now_ms;
// get system id and component id of offboard navigation system
uint8_t sysid;
uint8_t compid;
mavlink_channel_t chan;
if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_ONBOARD_CONTROLLER, sysid, compid, chan)) {
gcs().chan(chan-MAVLINK_COMM_0).send_set_position_target_global_int(sysid, compid, guided_target.loc);
}
}
}