Rover: rename sent_notification to send_notification

also fix init of this flag in guided mode to avoid sending two notifications
This commit is contained in:
Randy Mackay 2021-03-03 13:51:46 +09:00
parent 78ddd0512c
commit 29e85aa516
3 changed files with 9 additions and 9 deletions

View File

@ -430,7 +430,7 @@ protected:
bool have_attitude_target; // true if we have a valid attitude target bool have_attitude_target; // true if we have a valid attitude target
uint32_t _des_att_time_ms; // system time last call to set_desired_attitude was made (used for timeout) uint32_t _des_att_time_ms; // system time last call to set_desired_attitude was made (used for timeout)
float _desired_yaw_rate_cds;// target turn rate centi-degrees per second float _desired_yaw_rate_cds;// target turn rate centi-degrees per second
bool sent_notification; // used to send one time notification to ground station bool send_notification; // used to send one time notification to ground station
float _desired_speed; // desired speed used only in HeadingAndSpeed submode float _desired_speed; // desired speed used only in HeadingAndSpeed submode
// direct steering and throttle control // direct steering and throttle control
@ -553,7 +553,7 @@ protected:
bool _enter() override; bool _enter() override;
bool sent_notification; // used to send one time notification to ground station bool send_notification; // used to send one time notification to ground station
bool _loitering; // true if loitering at end of RTL bool _loitering; // true if loitering at end of RTL
}; };

View File

@ -12,7 +12,7 @@ bool ModeGuided::_enter()
// initialise waypoint speed // initialise waypoint speed
g2.wp_nav.set_desired_speed_to_default(); g2.wp_nav.set_desired_speed_to_default();
sent_notification = false; send_notification = false;
return true; return true;
} }
@ -28,8 +28,8 @@ void ModeGuided::update()
navigate_to_waypoint(); navigate_to_waypoint();
} else { } else {
// send notification // send notification
if (!sent_notification) { if (send_notification) {
sent_notification = true; send_notification = false;
rover.gcs().send_mission_item_reached_message(0); rover.gcs().send_mission_item_reached_message(0);
} }
@ -228,7 +228,7 @@ bool ModeGuided::set_desired_location(const struct Location& destination,
// handle guided specific initialisation and logging // handle guided specific initialisation and logging
_guided_mode = ModeGuided::Guided_WP; _guided_mode = ModeGuided::Guided_WP;
sent_notification = false; send_notification = true;
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f)); rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f));
return true; return true;
} }

View File

@ -27,7 +27,7 @@ bool ModeRTL::_enter()
g2.wp_nav.set_desired_speed_to_default(); g2.wp_nav.set_desired_speed_to_default();
} }
sent_notification = false; send_notification = true;
_loitering = false; _loitering = false;
return true; return true;
} }
@ -40,8 +40,8 @@ void ModeRTL::update()
navigate_to_waypoint(); navigate_to_waypoint();
} else { } else {
// send notification // send notification
if (!sent_notification) { if (send_notification) {
sent_notification = true; send_notification = false;
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination"); gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
} }