|
|
|
@ -325,28 +325,31 @@ void Rover::Log_Write_WheelEncoder()
@@ -325,28 +325,31 @@ void Rover::Log_Write_WheelEncoder()
|
|
|
|
|
DataFlash.WriteBlock(&pkt, sizeof(pkt)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// type and unit information can be found in
|
|
|
|
|
// libraries/DataFlash/Logstructure.h; search for "log_Units" for
|
|
|
|
|
// units and "Format characters" for field type information
|
|
|
|
|
const LogStructure Rover::log_structure[] = { |
|
|
|
|
LOG_COMMON_STRUCTURES, |
|
|
|
|
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance), |
|
|
|
|
"PM", "QIHIhhhBHI", "TimeUS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr,Mem" }, |
|
|
|
|
"PM", "QIHIhhhBHI", "TimeUS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr,Mem", "ss-------b", "FC-------0" }, |
|
|
|
|
{ LOG_STARTUP_MSG, sizeof(log_Startup), |
|
|
|
|
"STRT", "QBH", "TimeUS,SType,CTot" }, |
|
|
|
|
"STRT", "QBH", "TimeUS,SType,CTot", "s--", "F--" }, |
|
|
|
|
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning), |
|
|
|
|
"CTUN", "Qhcchf", "TimeUS,Steer,Roll,Pitch,ThrOut,AccY" }, |
|
|
|
|
"CTUN", "Qhcchf", "TimeUS,Steer,Roll,Pitch,ThrOut,AccY", "s-dd-o", "F-BB-0" }, |
|
|
|
|
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning), |
|
|
|
|
"NTUN", "QHfHHbf", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,Thr,XT" }, |
|
|
|
|
"NTUN", "QHfHHbf", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,Thr,XT", "sdmdd-m", "FB0BB-0" }, |
|
|
|
|
{ LOG_RANGEFINDER_MSG, sizeof(log_Rangefinder), |
|
|
|
|
"RGFD", "QfHHHbHCb", "TimeUS,LatAcc,R1Dist,R2Dist,DCnt,TAng,TTim,Spd,Thr" }, |
|
|
|
|
"RGFD", "QfHHHbHCb", "TimeUS,LatAcc,R1Dist,R2Dist,DCnt,TAng,TTim,Spd,Thr", "somm-hsm-", "F0BB-0CB-" }, |
|
|
|
|
{ LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm), |
|
|
|
|
"ARM", "QBH", "TimeUS,ArmState,ArmChecks" }, |
|
|
|
|
"ARM", "QBH", "TimeUS,ArmState,ArmChecks", "s--", "F--" }, |
|
|
|
|
{ LOG_STEERING_MSG, sizeof(log_Steering), |
|
|
|
|
"STER", "Qff", "TimeUS,Demanded,Achieved" }, |
|
|
|
|
"STER", "Qff", "TimeUS,Demanded,Achieved", "so?", "F0?" }, |
|
|
|
|
{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), |
|
|
|
|
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" }, |
|
|
|
|
"GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" }, |
|
|
|
|
{ LOG_ERROR_MSG, sizeof(log_Error), |
|
|
|
|
"ERR", "QBB", "TimeUS,Subsys,ECode" }, |
|
|
|
|
"ERR", "QBB", "TimeUS,Subsys,ECode", "s--", "F--" }, |
|
|
|
|
{ LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), |
|
|
|
|
"WENC", "Qfbfb", "TimeUS,Dist0,Qual0,Dist1,Qual1" }, |
|
|
|
|
"WENC", "Qfbfb", "TimeUS,Dist0,Qual0,Dist1,Qual1", "sm-m-", "F0-0-" }, |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
void Rover::log_init(void) |
|
|
|
|