dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MAVLink app: Fix VTOL reporting and prevent mission reached spam 

The VTOL status reporting and the mission status reporting were both suboptimal. VTOL was too slow, mission reporting too fast
This commit is contained in:
Lorenz Meier 2016-12-25 17:07:34 +01:00
parent 171ccd1203
commit 737e18dccb
3 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/mavlink/mavlink_main.cpp
View File

@ -2011,8 +2011,8 @@ Mavlink::task_main(int argc, char *argv[])
break;
case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("EXTENDED_SYS_STATE", 2.0f);
configure_stream("SYS_STATUS", 5.0f);
configure_stream("EXTENDED_SYS_STATE", 5.0f);
configure_stream("HIGHRES_IMU", 50.0f);
configure_stream("ATTITUDE", 250.0f);
configure_stream("RC_CHANNELS", 20.0f);

6
src/modules/mavlink/mavlink_mission.cpp
View File

@ -70,6 +70,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
_state(MAVLINK_WPM_STATE_IDLE),
_time_last_recv(0),
_time_last_sent(0),
_time_last_reached(0),
_action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT),
_retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT),
_int_mode(true),
@ -358,6 +359,7 @@ MavlinkMissionManager::send(const hrt_abstime now)
if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); }
if (mission_result.reached) {
_time_last_reached = now;
_last_reached = mission_result.seq_reached;
send_mission_item_reached((uint16_t)mission_result.seq_reached);
} else {
@ -375,7 +377,9 @@ MavlinkMissionManager::send(const hrt_abstime now)
} else {
if (_slow_rate_limiter.check(now)) {
send_mission_current(_current_seq);
if (_last_reached >= 0) {
// send the reached message a couple of times after reaching the waypoint
if (_last_reached >= 0 && (now - _time_last_reached) < 300 * 1000) {
send_mission_item_reached((uint16_t)_last_reached);
}
}

1
src/modules/mavlink/mavlink_mission.h
View File

@ -105,6 +105,7 @@ private:
uint64_t _time_last_recv;
uint64_t _time_last_sent;
uint64_t _time_last_reached; ///< last time when the vehicle reached a waypoint
uint32_t _action_timeout;
uint32_t _retry_timeout;