diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index b6fd5900a2..01a3daca69 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -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; } } diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 2b2e030059..2dffe06bc3 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -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 diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index b53df70b95..902522a11e 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -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); } } diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index 5261a89b51..40c8dda32b 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -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); }