Rover: added MAVLink SYSTEM_TIME message

This commit is contained in:
Andrew Tridgell 2013-10-23 22:15:37 +11:00
parent 92bf4b9c89
commit 7a9048f225
3 changed files with 19 additions and 2 deletions

View File

@ -246,6 +246,14 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
g_gps->num_sats);
}
static void NOINLINE send_system_time(mavlink_channel_t chan)
{
mavlink_msg_system_time_send(
chan,
g_gps->time_epoch_usec(),
hal.scheduler->millis());
}
#if HIL_MODE != HIL_MODE_DISABLED
static void NOINLINE send_servo_out(mavlink_channel_t chan)
@ -509,6 +517,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
send_gps_raw(chan);
break;
case MSG_SYSTEM_TIME:
CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
send_system_time(chan);
break;
case MSG_SERVO_OUT:
#if HIL_MODE != HIL_MODE_DISABLED
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
@ -951,6 +964,7 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_AHRS);
send_message(MSG_HWSTATUS);
send_message(MSG_RANGEFINDER);
send_message(MSG_SYSTEM_TIME);
}
}

View File

@ -228,6 +228,7 @@ static void Log_Write_Cmd(uint8_t num, const struct Location *wp)
struct PACKED log_Camera {
LOG_PACKET_HEADER;
uint32_t gps_time;
uint16_t gps_week;
int32_t latitude;
int32_t longitude;
int16_t roll;
@ -241,7 +242,8 @@ static void Log_Write_Camera()
#if CAMERA == ENABLED
struct log_Camera pkt = {
LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG),
gps_time : g_gps->time,
gps_time : g_gps->time_week_ms,
gps_week : g_gps->time_week,
latitude : current_loc.lat,
longitude : current_loc.lng,
roll : (int16_t)ahrs.roll_sensor,
@ -458,7 +460,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
{ LOG_CMD_MSG, sizeof(log_Cmd),
"CMD", "BBBBBeLL", "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
{ LOG_CAMERA_MSG, sizeof(log_Camera),
"CAM", "ILLccC", "GPSTime,Lat,Lng,Roll,Pitch,Yaw" },
"CAM", "IHLLeccC", "GPSTime,GPSWeek,Lat,Lng,Alt,Roll,Pitch,Yaw" },
{ LOG_STARTUP_MSG, sizeof(log_Startup),
"STRT", "BB", "SType,CTot" },
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),

View File

@ -101,6 +101,7 @@ enum ap_message {
MSG_RAW_IMU1,
MSG_RAW_IMU3,
MSG_GPS_RAW,
MSG_SYSTEM_TIME,
MSG_SERVO_OUT,
MSG_NEXT_WAYPOINT,
MSG_NEXT_PARAM,