Rover: guided-within-auto target moved to structure

also guided's limit_breached made const
also guided limit check uses is_positive
This commit is contained in:
Randy Mackay 2019-03-15 13:22:36 +09:00
parent 89f0418c0f
commit 3f8adb4e7d
4 changed files with 20 additions and 18 deletions

View File

@ -325,8 +325,8 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
}
// if a location target was set, return true once vehicle is close
if (guided_target_valid) {
if (rover.current_loc.get_distance(guided_target) <= rover.g.waypoint_radius) {
if (guided_target.valid) {
if (rover.current_loc.get_distance(guided_target.loc) <= rover.g.waypoint_radius) {
return true;
}
}

View File

@ -340,10 +340,12 @@ private:
uint32_t loiter_start_time; // How long have we been loitering - The start time in millis
bool previously_reached_wp; // set to true if we have EVER reached the waypoint
// Guided variables
Location guided_target; // location target sent to external navigation
bool guided_target_valid; // true if guided_target is valid
uint32_t guided_target_sent_ms; // system time that target was last sent to offboard navigation
// Guided-within-Auto variables
struct {
Location loc; // location target sent to external navigation
bool valid; // true if loc is valid
uint32_t last_sent_ms; // system time that target was last sent to offboard navigation
} guided_target;
// Conditional command
// A value used in condition commands (eg delay, change alt, etc.)
@ -393,7 +395,7 @@ public:
void limit_set(uint32_t timeout_ms, float horiz_max);
void limit_clear();
void limit_init_time_and_location();
bool limit_breached();
bool limit_breached() const;
protected:
@ -414,8 +416,8 @@ protected:
float _desired_yaw_rate_cds;// target turn rate centi-degrees per second
// limits
struct Guided_Limit {
uint32_t timeout_ms;// timeout (in seconds) from the time that guided is invoked
struct {
uint32_t timeout_ms;// timeout from the time that guided is invoked
float horiz_max; // horizontal position limit in meters from where guided mode was initiated (0 = no limit)
uint32_t start_time;// system time in milliseconds that control was handed to the external computer
Location start_loc; // starting location for checking horiz_max limit

View File

@ -190,10 +190,10 @@ void ModeAuto::start_guided(const Location& loc)
Location loc_sanitised = loc;
if ((loc_sanitised.lat != 0) || (loc_sanitised.lng != 0)) {
location_sanitize(rover.current_loc, loc_sanitised);
guided_target = loc_sanitised;
guided_target_valid = true;
guided_target.loc = loc_sanitised;
guided_target.valid = true;
} else {
guided_target_valid = false;
guided_target.valid = false;
}
}
}
@ -201,21 +201,21 @@ void ModeAuto::start_guided(const Location& loc)
// send latest position target to offboard navigation system
void ModeAuto::send_guided_position_target()
{
if (!guided_target_valid) {
if (!guided_target.valid) {
return;
}
// send at maximum of 1hz
uint32_t now_ms = AP_HAL::millis();
if ((guided_target_sent_ms == 0) || (now_ms - guided_target_sent_ms > AUTO_GUIDED_SEND_TARGET_MS)) {
guided_target_sent_ms = now_ms;
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 = 0;
uint8_t compid = 0;
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);
gcs().chan(chan-MAVLINK_COMM_0).send_set_position_target_global_int(sysid, compid, guided_target.loc);
}
}

View File

@ -225,7 +225,7 @@ void ModeGuided::limit_init_time_and_location()
}
// returns true if guided mode has breached a limit
bool ModeGuided::limit_breached()
bool ModeGuided::limit_breached() const
{
// check if we have passed the timeout
if ((limit.timeout_ms > 0) && (millis() - limit.start_time >= limit.timeout_ms)) {
@ -233,7 +233,7 @@ bool ModeGuided::limit_breached()
}
// check if we have gone beyond horizontal limit
if (limit.horiz_max > 0.0f) {
if (is_positive(limit.horiz_max)) {
return (rover.current_loc.get_distance(limit.start_loc) > limit.horiz_max);
}