diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 2e5994b8a9..5cdd7d782a 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -162,6 +162,7 @@ struct PACKED log_Format { struct PACKED log_Parameter { LOG_PACKET_HEADER; + uint64_t time_us; char name[16]; float value; }; @@ -203,12 +204,13 @@ struct PACKED log_GPS2 { struct PACKED log_Message { LOG_PACKET_HEADER; + uint64_t time_us; char msg[64]; }; struct PACKED log_IMU { LOG_PACKET_HEADER; - uint32_t timestamp; + uint64_t time_us; float gyro_x, gyro_y, gyro_z; float accel_x, accel_y, accel_z; uint32_t gyro_error, accel_error; @@ -218,7 +220,7 @@ struct PACKED log_IMU { struct PACKED log_RCIN { LOG_PACKET_HEADER; - uint32_t timestamp; + uint64_t time_us; uint16_t chan1; uint16_t chan2; uint16_t chan3; @@ -237,7 +239,7 @@ struct PACKED log_RCIN { struct PACKED log_RCOUT { LOG_PACKET_HEADER; - uint32_t timestamp; + uint64_t time_us; uint16_t chan1; uint16_t chan2; uint16_t chan3; @@ -254,7 +256,7 @@ struct PACKED log_RCOUT { struct PACKED log_BARO { LOG_PACKET_HEADER; - uint32_t timestamp; + uint64_t time_us; float altitude; float pressure; int16_t temperature; @@ -263,7 +265,7 @@ struct PACKED log_BARO { struct PACKED log_AHRS { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; int16_t roll; int16_t pitch; uint16_t yaw; @@ -274,7 +276,7 @@ struct PACKED log_AHRS { struct PACKED log_POS { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; int32_t lat; int32_t lng; float alt; @@ -283,7 +285,7 @@ struct PACKED log_POS { struct PACKED log_POWR { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; uint16_t Vcc; uint16_t Vservo; uint16_t flags; @@ -291,7 +293,7 @@ struct PACKED log_POWR { struct PACKED log_EKF1 { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; int16_t roll; int16_t pitch; uint16_t yaw; @@ -308,7 +310,7 @@ struct PACKED log_EKF1 { struct PACKED log_EKF2 { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; int8_t Ratio; int8_t AZ1bias; int8_t AZ2bias; @@ -324,7 +326,7 @@ struct PACKED log_EKF2 { struct PACKED log_EKF3 { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; int16_t innovVN; int16_t innovVE; int16_t innovVD; @@ -339,7 +341,7 @@ struct PACKED log_EKF3 { struct PACKED log_EKF4 { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; int16_t sqrtvarV; int16_t sqrtvarP; int16_t sqrtvarH; @@ -356,7 +358,7 @@ struct PACKED log_EKF4 { struct PACKED log_EKF5 { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; uint8_t normInnov; int16_t FIX; int16_t FIY; @@ -370,7 +372,7 @@ struct PACKED log_EKF5 { struct PACKED log_Cmd { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; uint16_t command_total; uint16_t sequence; uint16_t command; @@ -385,7 +387,7 @@ struct PACKED log_Cmd { struct PACKED log_Radio { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; uint8_t rssi; uint8_t remrssi; uint8_t txbuf; @@ -410,7 +412,7 @@ struct PACKED log_Camera { struct PACKED log_Attitude { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; int16_t control_roll; int16_t roll; int16_t control_pitch; @@ -423,7 +425,7 @@ struct PACKED log_Attitude { struct PACKED log_Current { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; int16_t throttle; int16_t battery_voltage; int16_t current_amps; @@ -434,7 +436,7 @@ struct PACKED log_Current { struct PACKED log_Compass { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; int16_t mag_x; int16_t mag_y; int16_t mag_z; @@ -449,7 +451,7 @@ struct PACKED log_Compass { struct PACKED log_Mode { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; uint8_t mode; uint8_t mode_num; }; @@ -459,7 +461,7 @@ struct PACKED log_Mode { */ struct PACKED log_TERRAIN { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; uint8_t status; int32_t lat; int32_t lng; @@ -475,7 +477,7 @@ struct PACKED log_TERRAIN { */ struct PACKED log_Ubx1 { LOG_PACKET_HEADER; - uint32_t timestamp; + uint64_t time_us; uint8_t instance; uint16_t noisePerMS; uint8_t jamInd; @@ -485,7 +487,7 @@ struct PACKED log_Ubx1 { struct PACKED log_Ubx2 { LOG_PACKET_HEADER; - uint32_t timestamp; + uint64_t time_us; uint8_t instance; int8_t ofsI; uint8_t magI; @@ -495,7 +497,7 @@ struct PACKED log_Ubx2 { struct PACKED log_Ubx3 { LOG_PACKET_HEADER; - uint32_t timestamp; + uint64_t time_us; uint8_t instance; float hAcc; float vAcc; @@ -504,7 +506,7 @@ struct PACKED log_Ubx3 { struct PACKED log_GPS_RAW { LOG_PACKET_HEADER; - uint32_t timestamp; + uint64_t time_us; int32_t iTOW; int16_t week; uint8_t numSV; @@ -519,7 +521,7 @@ struct PACKED log_GPS_RAW { struct PACKED log_Esc { LOG_PACKET_HEADER; - uint32_t time_ms; + uint64_t time_us; int16_t rpm; int16_t voltage; int16_t current; @@ -528,7 +530,7 @@ struct PACKED log_Esc { struct PACKED log_AIRSPEED { LOG_PACKET_HEADER; - uint32_t timestamp; + uint64_t time_us; float airspeed; float diffpressure; int16_t temperature; @@ -569,6 +571,8 @@ Format characters in the format string for binary log messages E : uint32_t * 100 L : int32_t latitude/longitude M : uint8_t flight mode + q : int64_t + Q : uint64_t */ // messages for all boards @@ -576,94 +580,94 @@ Format characters in the format string for binary log messages { LOG_FORMAT_MSG, sizeof(log_Format), \ "FMT", "BBnNZ", "Type,Length,Name,Format,Columns" }, \ { LOG_PARAMETER_MSG, sizeof(log_Parameter), \ - "PARM", "Nf", "Name,Value" }, \ + "PARM", "QNf", "TimeUS,Name,Value" }, \ { LOG_GPS_MSG, sizeof(log_GPS), \ "GPS", "BIHBcLLeeEefI", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs,VZ,T" }, \ { LOG_IMU_MSG, sizeof(log_IMU), \ - "IMU", "IffffffIIfBB", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp,GyHlt,AcHlt" }, \ + "IMU", "QffffffIIfBB", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp,GyHlt,AcHlt" }, \ { LOG_MESSAGE_MSG, sizeof(log_Message), \ - "MSG", "Z", "Message"}, \ + "MSG", "QZ", "TimeUS,Message"}, \ { LOG_RCIN_MSG, sizeof(log_RCIN), \ - "RCIN", "Ihhhhhhhhhhhhhh", "TimeMS,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,C12,C13,C14" }, \ + "RCIN", "Qhhhhhhhhhhhhhh", "TimeUS,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,C12,C13,C14" }, \ { LOG_RCOUT_MSG, sizeof(log_RCOUT), \ - "RCOU", "Ihhhhhhhhhhhh", "TimeMS,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Ch8,Ch9,Ch10,Ch11,Ch12" }, \ + "RCOU", "Qhhhhhhhhhhhh", "TimeUS,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Ch8,Ch9,Ch10,Ch11,Ch12" }, \ { LOG_BARO_MSG, sizeof(log_BARO), \ - "BARO", "Iffcf", "TimeMS,Alt,Press,Temp,CRt" }, \ + "BARO", "Qffcf", "TimeUS,Alt,Press,Temp,CRt" }, \ { LOG_BAR2_MSG, sizeof(log_BARO), \ - "BAR2", "Iffcf", "TimeMS,Alt,Press,Temp,CRt" }, \ + "BAR2", "Qffcf", "TimeUS,Alt,Press,Temp,CRt" }, \ { LOG_POWR_MSG, sizeof(log_POWR), \ - "POWR","ICCH","TimeMS,Vcc,VServo,Flags" }, \ + "POWR","QCCH","TimeUS,Vcc,VServo,Flags" }, \ { LOG_CMD_MSG, sizeof(log_Cmd), \ - "CMD", "IHHHfffffff","TimeMS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt" }, \ + "CMD", "QHHHfffffff","TimeUS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt" }, \ { LOG_RADIO_MSG, sizeof(log_Radio), \ - "RAD", "IBBBBBHH", "TimeMS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed" }, \ + "RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed" }, \ { LOG_CAMERA_MSG, sizeof(log_Camera), \ "CAM", "IHLLeeccC","GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,Roll,Pitch,Yaw" }, \ { LOG_ARSP_MSG, sizeof(log_AIRSPEED), \ - "ARSP", "Iffcff", "TimeMS,Airspeed,DiffPress,Temp,RawPress,Offset" }, \ + "ARSP", "Qffcff", "TimeUS,Airspeed,DiffPress,Temp,RawPress,Offset" }, \ { LOG_CURRENT_MSG, sizeof(log_Current), \ - "CURR", "IhhhHfh","TimeMS,Throttle,Volt,Curr,Vcc,CurrTot,Volt2" },\ + "CURR", "QhhhHfh","TimeUS,Throttle,Volt,Curr,Vcc,CurrTot,Volt2" },\ { LOG_ATTITUDE_MSG, sizeof(log_Attitude),\ - "ATT", "IccccCCCC", "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw" }, \ + "ATT", "QccccCCCC", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw" }, \ { LOG_COMPASS_MSG, sizeof(log_Compass), \ - "MAG", "IhhhhhhhhhB", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ,Health" }, \ + "MAG", "QhhhhhhhhhB", "TimeUS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ,Health" }, \ { LOG_MODE_MSG, sizeof(log_Mode), \ - "MODE", "IMB", "TimeMS,Mode,ModeNum" } + "MODE", "QMB", "TimeUS,Mode,ModeNum" } // messages for more advanced boards #define LOG_EXTRA_STRUCTURES \ { LOG_GPS2_MSG, sizeof(log_GPS2), \ "GPS2", "BIHBcLLeEefIBI", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,T,DSc,DAg" }, \ { LOG_IMU2_MSG, sizeof(log_IMU), \ - "IMU2", "IffffffIIfBB", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp,GyHlt,AcHlt" }, \ + "IMU2", "QffffffIIfBB", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp,GyHlt,AcHlt" }, \ { LOG_IMU3_MSG, sizeof(log_IMU), \ - "IMU3", "IffffffIIfBB", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp,GyHlt,AcHlt" }, \ + "IMU3", "QffffffIIfBB", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp,GyHlt,AcHlt" }, \ { LOG_AHR2_MSG, sizeof(log_AHRS), \ - "AHR2","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \ + "AHR2","QccCfLL","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \ { LOG_POS_MSG, sizeof(log_POS), \ - "POS","ILLff","TimeMS,Lat,Lng,Alt,RelAlt" }, \ + "POS","QLLff","TimeUS,Lat,Lng,Alt,RelAlt" }, \ { LOG_SIMSTATE_MSG, sizeof(log_AHRS), \ - "SIM","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \ + "SIM","QccCfLL","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \ { LOG_EKF1_MSG, sizeof(log_EKF1), \ - "EKF1","IccCffffffccc","TimeMS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, \ + "EKF1","QccCffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, \ { LOG_EKF2_MSG, sizeof(log_EKF2), \ - "EKF2","Ibbbcchhhhhh","TimeMS,Ratio,AZ1bias,AZ2bias,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \ + "EKF2","Qbbbcchhhhhh","TimeUS,Ratio,AZ1bias,AZ2bias,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \ { LOG_EKF3_MSG, sizeof(log_EKF3), \ - "EKF3","Icccccchhhc","TimeMS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \ + "EKF3","Qcccccchhhc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \ { LOG_EKF4_MSG, sizeof(log_EKF4), \ - "EKF4","IcccccccbbBBH","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS" }, \ + "EKF4","QcccccccbbBBH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS" }, \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ - "TERR","IBLLHffHH","TimeMS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \ + "TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \ { LOG_UBX1_MSG, sizeof(log_Ubx1), \ - "UBX1", "IBHBBH", "TimeMS,Instance,noisePerMS,jamInd,aPower,agcCnt" }, \ + "UBX1", "QBHBBH", "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt" }, \ { LOG_UBX2_MSG, sizeof(log_Ubx2), \ - "UBX2", "IBbBbB", "TimeMS,Instance,ofsI,magI,ofsQ,magQ" }, \ + "UBX2", "QBbBbB", "TimeUS,Instance,ofsI,magI,ofsQ,magQ" }, \ { LOG_UBX3_MSG, sizeof(log_Ubx3), \ - "UBX3", "IBfff", "TimeMS,Instance,hAcc,vAcc,sAcc" }, \ + "UBX3", "QBfff", "TimeUS,Instance,hAcc,vAcc,sAcc" }, \ { LOG_GPS_RAW_MSG, sizeof(log_GPS_RAW), \ - "GRAW", "IIHBBddfBbB", "TimeMS,WkMS,Week,numSV,sv,cpMes,prMes,doMes,mesQI,cno,lli" }, \ + "GRAW", "QIHBBddfBbB", "TimeUS,WkMS,Week,numSV,sv,cpMes,prMes,doMes,mesQI,cno,lli" }, \ { LOG_ESC1_MSG, sizeof(log_Esc), \ - "ESC1", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \ + "ESC1", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \ { LOG_ESC2_MSG, sizeof(log_Esc), \ - "ESC2", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \ + "ESC2", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \ { LOG_ESC3_MSG, sizeof(log_Esc), \ - "ESC3", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \ + "ESC3", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \ { LOG_ESC4_MSG, sizeof(log_Esc), \ - "ESC4", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \ + "ESC4", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \ { LOG_ESC5_MSG, sizeof(log_Esc), \ - "ESC5", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \ + "ESC5", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \ { LOG_ESC6_MSG, sizeof(log_Esc), \ - "ESC6", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \ + "ESC6", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \ { LOG_ESC7_MSG, sizeof(log_Esc), \ - "ESC7", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \ + "ESC7", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \ { LOG_ESC8_MSG, sizeof(log_Esc), \ - "ESC8", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \ + "ESC8", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \ { LOG_EKF5_MSG, sizeof(log_EKF5), \ - "EKF5","IBhhhcccCC","TimeMS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \ + "EKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \ { LOG_COMPASS2_MSG, sizeof(log_Compass), \ - "MAG2","IhhhhhhhhhB", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ,Health" }, \ + "MAG2","QhhhhhhhhhB", "TimeUS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ,Health" }, \ { LOG_COMPASS3_MSG, sizeof(log_Compass), \ - "MAG3","IhhhhhhhhhB", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ,Health" }, \ + "MAG3","QhhhhhhhhhB", "TimeUS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ,Health" }, \ { LOG_ACC1_MSG, sizeof(log_ACCEL), \ "ACC1", "IIfff", "TimeMS,SampleUS,AccX,AccY,AccZ" }, \ { LOG_ACC2_MSG, sizeof(log_ACCEL), \ diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 97d5dd0b35..1499fe9080 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -621,6 +621,7 @@ void DataFlash_Class::Log_Write_Parameter(const char *name, float value) { struct log_Parameter pkt = { LOG_PACKET_HEADER_INIT(LOG_PARAMETER_MSG), + time_us : hal.scheduler->micros64(), name : {}, value : value }; @@ -711,10 +712,9 @@ void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i, int32_t relati // Write an RCIN packet void DataFlash_Class::Log_Write_RCIN(void) { - uint32_t timestamp = hal.scheduler->millis(); struct log_RCIN pkt = { LOG_PACKET_HEADER_INIT(LOG_RCIN_MSG), - timestamp : timestamp, + time_us : hal.scheduler->micros64(), chan1 : hal.rcin->read(0), chan2 : hal.rcin->read(1), chan3 : hal.rcin->read(2), @@ -738,7 +738,7 @@ void DataFlash_Class::Log_Write_RCOUT(void) { struct log_RCOUT pkt = { LOG_PACKET_HEADER_INIT(LOG_RCOUT_MSG), - timestamp : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), chan1 : hal.rcout->read(0), chan2 : hal.rcout->read(1), chan3 : hal.rcout->read(2), @@ -759,10 +759,10 @@ void DataFlash_Class::Log_Write_RCOUT(void) // Write a BARO packet void DataFlash_Class::Log_Write_Baro(AP_Baro &baro) { - uint32_t now = hal.scheduler->millis(); + uint64_t time_us = hal.scheduler->micros64(); struct log_BARO pkt = { LOG_PACKET_HEADER_INIT(LOG_BARO_MSG), - timestamp : now, + time_us : time_us, altitude : baro.get_altitude(0), pressure : baro.get_pressure(0), temperature : (int16_t)(baro.get_temperature(0) * 100), @@ -773,7 +773,7 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro) if (baro.num_instances() > 1 && baro.healthy(1)) { struct log_BARO pkt2 = { LOG_PACKET_HEADER_INIT(LOG_BAR2_MSG), - timestamp : now, + time_us : time_us, altitude : baro.get_altitude(1), pressure : baro.get_pressure(1), temperature : (int16_t)(baro.get_temperature(1) * 100), @@ -787,12 +787,12 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro) // Write an raw accel/gyro data packet void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) { - uint32_t tstamp = hal.scheduler->millis(); + uint64_t time_us = hal.scheduler->micros64(); const Vector3f &gyro = ins.get_gyro(0); const Vector3f &accel = ins.get_accel(0); struct log_IMU pkt = { LOG_PACKET_HEADER_INIT(LOG_IMU_MSG), - timestamp : tstamp, + time_us : time_us, gyro_x : gyro.x, gyro_y : gyro.y, gyro_z : gyro.z, @@ -814,7 +814,7 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) const Vector3f &accel2 = ins.get_accel(1); struct log_IMU pkt2 = { LOG_PACKET_HEADER_INIT(LOG_IMU2_MSG), - timestamp : tstamp, + time_us : time_us, gyro_x : gyro2.x, gyro_y : gyro2.y, gyro_z : gyro2.z, @@ -835,7 +835,7 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) const Vector3f &accel3 = ins.get_accel(2); struct log_IMU pkt3 = { LOG_PACKET_HEADER_INIT(LOG_IMU3_MSG), - timestamp : tstamp, + time_us : time_us, gyro_x : gyro3.x, gyro_y : gyro3.y, gyro_z : gyro3.z, @@ -857,6 +857,7 @@ void DataFlash_Class::Log_Write_Message(const char *message) { struct log_Message pkt = { LOG_PACKET_HEADER_INIT(LOG_MESSAGE_MSG), + time_us : hal.scheduler->micros64(), msg : {} }; strncpy(pkt.msg, message, sizeof(pkt.msg)); @@ -868,6 +869,7 @@ void DataFlash_Class::Log_Write_Message_P(const prog_char_t *message) { struct log_Message pkt = { LOG_PACKET_HEADER_INIT(LOG_MESSAGE_MSG), + time_us : hal.scheduler->micros64(), msg : {} }; strncpy_P(pkt.msg, message, sizeof(pkt.msg)); @@ -880,7 +882,7 @@ void DataFlash_Class::Log_Write_Power(void) #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 struct log_POWR pkt = { LOG_PACKET_HEADER_INIT(LOG_POWR_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), Vcc : (uint16_t)(hal.analogin->board_voltage() * 100), Vservo : (uint16_t)(hal.analogin->servorail_voltage() * 100), flags : hal.analogin->power_status_flags() @@ -899,7 +901,7 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs) } struct log_AHRS pkt = { LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), roll : (int16_t)(degrees(euler.x)*100), pitch : (int16_t)(degrees(euler.y)*100), yaw : (uint16_t)(wrap_360_cd(degrees(euler.z)*100)), @@ -921,7 +923,7 @@ void DataFlash_Class::Log_Write_POS(AP_AHRS &ahrs) ahrs.get_relative_position_NED(pos); struct log_POS pkt = { LOG_PACKET_HEADER_INIT(LOG_POS_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), lat : loc.lat, lng : loc.lng, alt : loc.alt*1.0e-2f, @@ -946,7 +948,7 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) ahrs.get_NavEKF().getGyroBias(gyroBias); struct log_EKF1 pkt = { LOG_PACKET_HEADER_INIT(LOG_EKF1_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string) pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string) yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string) @@ -975,7 +977,7 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) ahrs.get_NavEKF().getMagXYZ(magXYZ); struct log_EKF2 pkt2 = { LOG_PACKET_HEADER_INIT(LOG_EKF2_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), Ratio : (int8_t)(100*ratio), AZ1bias : (int8_t)(100*az1bias), AZ2bias : (int8_t)(100*az2bias), @@ -998,7 +1000,7 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) ahrs.get_NavEKF().getInnovations(velInnov, posInnov, magInnov, tasInnov); struct log_EKF3 pkt3 = { LOG_PACKET_HEADER_INIT(LOG_EKF3_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), innovVN : (int16_t)(100*velInnov.x), innovVE : (int16_t)(100*velInnov.y), innovVD : (int16_t)(100*velInnov.z), @@ -1027,7 +1029,7 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) ahrs.get_NavEKF().getFilterStatus(solutionStatus); struct log_EKF4 pkt4 = { LOG_PACKET_HEADER_INIT(LOG_EKF4_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), sqrtvarV : (int16_t)(100*velVar), sqrtvarP : (int16_t)(100*posVar), sqrtvarH : (int16_t)(100*hgtVar), @@ -1057,7 +1059,7 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) ahrs.get_NavEKF().getFlowDebug(normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr); struct log_EKF5 pkt5 = { LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), normInnov : (uint8_t)(min(100*normInnov,255)), FIX : (int16_t)(1000*flowInnovX), FIY : (int16_t)(1000*flowInnovY), @@ -1078,7 +1080,7 @@ void DataFlash_Class::Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission { struct log_Cmd pkt = { LOG_PACKET_HEADER_INIT(LOG_CMD_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), command_total : (uint16_t)cmd_total, sequence : (uint16_t)mav_cmd.seq, command : (uint16_t)mav_cmd.command, @@ -1097,7 +1099,7 @@ void DataFlash_Class::Log_Write_Radio(const mavlink_radio_t &packet) { struct log_Radio pkt = { LOG_PACKET_HEADER_INIT(LOG_RADIO_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), rssi : packet.rssi, remrssi : packet.remrssi, txbuf : packet.txbuf, @@ -1141,7 +1143,7 @@ void DataFlash_Class::Log_Write_Attitude(AP_AHRS &ahrs, const Vector3f &targets) { struct log_Attitude pkt = { LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), control_roll : (int16_t)targets.x, roll : (int16_t)ahrs.roll_sensor, control_pitch : (int16_t)targets.y, @@ -1160,7 +1162,7 @@ void DataFlash_Class::Log_Write_Current(const AP_BattMonitor &battery, int16_t t float voltage2 = battery.voltage2(); struct log_Current pkt = { LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), throttle : throttle, battery_voltage : (int16_t) (battery.voltage() * 100.0f), current_amps : (int16_t) (battery.current_amps() * 100.0f), @@ -1179,7 +1181,7 @@ void DataFlash_Class::Log_Write_Compass(const Compass &compass) const Vector3f &mag_motor_offsets = compass.get_motor_offsets(0); struct log_Compass pkt = { LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), mag_x : (int16_t)mag_field.x, mag_y : (int16_t)mag_field.y, mag_z : (int16_t)mag_field.z, @@ -1200,7 +1202,7 @@ void DataFlash_Class::Log_Write_Compass(const Compass &compass) const Vector3f &mag_motor_offsets2 = compass.get_motor_offsets(1); struct log_Compass pkt2 = { LOG_PACKET_HEADER_INIT(LOG_COMPASS2_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), mag_x : (int16_t)mag_field2.x, mag_y : (int16_t)mag_field2.y, mag_z : (int16_t)mag_field2.z, @@ -1222,7 +1224,7 @@ void DataFlash_Class::Log_Write_Compass(const Compass &compass) const Vector3f &mag_motor_offsets3 = compass.get_motor_offsets(2); struct log_Compass pkt3 = { LOG_PACKET_HEADER_INIT(LOG_COMPASS3_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), mag_x : (int16_t)mag_field3.x, mag_y : (int16_t)mag_field3.y, mag_z : (int16_t)mag_field3.z, @@ -1244,7 +1246,7 @@ void DataFlash_Class::Log_Write_Mode(uint8_t mode) { struct log_Mode pkt = { LOG_PACKET_HEADER_INIT(LOG_MODE_MSG), - time_ms : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), mode : mode, mode_num : mode }; @@ -1270,7 +1272,7 @@ void DataFlash_Class::Log_Write_ESC(void) if (esc_status.esc_count > 8) { esc_status.esc_count = 8; } - uint32_t time_ms = hal.scheduler->millis(); + uint64_t time_us = hal.scheduler->micros64(); for (uint8_t i = 0; i < esc_status.esc_count; i++) { // skip logging ESCs with a esc_address of zero, and this // are probably not populated. The Pixhawk itself should @@ -1278,7 +1280,7 @@ void DataFlash_Class::Log_Write_ESC(void) if (esc_status.esc[i].esc_address != 0) { struct log_Esc pkt = { LOG_PACKET_HEADER_INIT((uint8_t)(LOG_ESC1_MSG + i)), - time_ms : time_ms, + time_us : time_us, rpm : (int16_t)(esc_status.esc[i].esc_rpm/10), voltage : (int16_t)(esc_status.esc[i].esc_voltage*100.0f + .5f), current : (int16_t)(esc_status.esc[i].esc_current*100.0f + .5f), @@ -1301,7 +1303,7 @@ void DataFlash_Class::Log_Write_Airspeed(AP_Airspeed &airspeed) } struct log_AIRSPEED pkt = { LOG_PACKET_HEADER_INIT(LOG_ARSP_MSG), - timestamp : hal.scheduler->millis(), + time_us : hal.scheduler->micros64(), airspeed : airspeed.get_raw_airspeed(), diffpressure : airspeed.get_differential_pressure(), temperature : (int16_t)(temperature * 100.0f),