|
|
|
@ -301,6 +301,11 @@ uint32_t GCS_MAVLINK_Rover::telem_delay() const
@@ -301,6 +301,11 @@ uint32_t GCS_MAVLINK_Rover::telem_delay() const
|
|
|
|
|
return static_cast<uint32_t>(rover.g.telem_delay); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool GCS_MAVLINK_Rover::vehicle_initialised() const |
|
|
|
|
{ |
|
|
|
|
return rover.control_mode != &rover.mode_initializing; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// try to send a message, return false if it won't fit in the serial tx buffer
|
|
|
|
|
bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) |
|
|
|
|
{ |
|
|
|
@ -319,17 +324,14 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
@@ -319,17 +324,14 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
|
|
|
|
|
|
|
|
|
|
switch (id) { |
|
|
|
|
|
|
|
|
|
case MSG_EXTENDED_STATUS1: |
|
|
|
|
case MSG_SYS_STATUS: |
|
|
|
|
// send extended status only once vehicle has been initialised
|
|
|
|
|
// to avoid unnecessary errors being reported to user
|
|
|
|
|
if (initialised) { |
|
|
|
|
if (PAYLOAD_SIZE(chan, SYS_STATUS) + |
|
|
|
|
PAYLOAD_SIZE(chan, POWER_STATUS) > comm_get_txspace(chan)) { |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
rover.send_sys_status(chan); |
|
|
|
|
send_power_status(); |
|
|
|
|
if (!vehicle_initialised()) { |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
CHECK_PAYLOAD_SIZE(SYS_STATUS); |
|
|
|
|
rover.send_sys_status(chan); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MSG_NAV_CONTROLLER_OUTPUT: |
|
|
|
@ -479,7 +481,8 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = {
@@ -479,7 +481,8 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = {
|
|
|
|
|
MSG_SENSOR_OFFSETS |
|
|
|
|
}; |
|
|
|
|
static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { |
|
|
|
|
MSG_EXTENDED_STATUS1, // SYS_STATUS, POWER_STATUS
|
|
|
|
|
MSG_SYS_STATUS, |
|
|
|
|
MSG_POWER_STATUS, |
|
|
|
|
MSG_MEMINFO, |
|
|
|
|
MSG_CURRENT_WAYPOINT, |
|
|
|
|
MSG_GPS_RAW, |
|
|
|
@ -1178,7 +1181,7 @@ void Rover::mavlink_delay_cb()
@@ -1178,7 +1181,7 @@ void Rover::mavlink_delay_cb()
|
|
|
|
|
if (tnow - last_1hz > 1000) { |
|
|
|
|
last_1hz = tnow; |
|
|
|
|
gcs().send_message(MSG_HEARTBEAT); |
|
|
|
|
gcs().send_message(MSG_EXTENDED_STATUS1); |
|
|
|
|
gcs().send_message(MSG_SYS_STATUS); |
|
|
|
|
} |
|
|
|
|
if (tnow - last_50hz > 20) { |
|
|
|
|
last_50hz = tnow; |
|
|
|
|