Browse Source

DataFlash: Minor whitespace changes/cleanup. No effect.

mission-4.1.18
Robert Lefebvre 10 years ago committed by Randy Mackay
parent
commit
be803f4f39
  1. 10
      libraries/DataFlash/DataFlash.h
  2. 64
      libraries/DataFlash/LogFile.cpp

10
libraries/DataFlash/DataFlash.h

@ -554,16 +554,16 @@ Format characters in the format string for binary log messages
"CAM", "IHLLeeccC","GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,Roll,Pitch,Yaw" }, \ "CAM", "IHLLeeccC","GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,Roll,Pitch,Yaw" }, \
{ LOG_ARSP_MSG, sizeof(log_AIRSPEED), \ { LOG_ARSP_MSG, sizeof(log_AIRSPEED), \
"ARSP", "Iffcff", "TimeMS,Airspeed,DiffPress,Temp,RawPress,Offset" } "ARSP", "Iffcff", "TimeMS,Airspeed,DiffPress,Temp,RawPress,Offset" }
{ LOG_CURRENT_MSG, sizeof(log_Current), \ { LOG_CURRENT_MSG, sizeof(log_Current), \
"CURR", "Ihhhhfh","TimeMS,Throttle,Volt,Curr,Vcc,CurrTot,Volt2" },\ "CURR", "Ihhhhfh","TimeMS,Throttle,Volt,Curr,Vcc,CurrTot,Volt2" },\
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),\ { LOG_ATTITUDE_MSG, sizeof(log_Attitude),\
"ATT", "IccccCCCC", "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw" },\ "ATT", "IccccCCCC", "TimeMS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw" }, \
{ LOG_COMPASS_MSG, sizeof(log_Compass), \ { LOG_COMPASS_MSG, sizeof(log_Compass), \
"MAG", "Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },\ "MAG", "Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" }, \
{ LOG_COMPASS2_MSG, sizeof(log_Compass), \ { LOG_COMPASS2_MSG, sizeof(log_Compass), \
"MAG2","Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },\ "MAG2","Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" }, \
{ LOG_COMPASS3_MSG, sizeof(log_Compass), \ { LOG_COMPASS3_MSG, sizeof(log_Compass), \
"MAG3","Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },\ "MAG3","Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" }, \
{ LOG_MODE_MSG, sizeof(log_Mode), \ { LOG_MODE_MSG, sizeof(log_Mode), \
"MODE", "IMB", "TimeMS,Mode,ModeNum" } "MODE", "IMB", "TimeMS,Mode,ModeNum" }

64
libraries/DataFlash/LogFile.cpp

@ -12,7 +12,6 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
void DataFlash_Class::Init(const struct LogStructure *structure, uint8_t num_types) void DataFlash_Class::Init(const struct LogStructure *structure, uint8_t num_types)
{ {
_num_types = num_types; _num_types = num_types;
@ -20,7 +19,6 @@ void DataFlash_Class::Init(const struct LogStructure *structure, uint8_t num_typ
_writes_enabled = true; _writes_enabled = true;
} }
// This function determines the number of whole or partial log files in the DataFlash // This function determines the number of whole or partial log files in the DataFlash
// Wholly overwritten files are (of course) lost. // Wholly overwritten files are (of course) lost.
uint16_t DataFlash_Block::get_num_logs(void) uint16_t DataFlash_Block::get_num_logs(void)
@ -56,7 +54,6 @@ uint16_t DataFlash_Block::get_num_logs(void)
return (last - first + 1); return (last - first + 1);
} }
// This function starts a new log file in the DataFlash // This function starts a new log file in the DataFlash
uint16_t DataFlash_Block::start_new_log(void) uint16_t DataFlash_Block::start_new_log(void)
{ {
@ -171,7 +168,6 @@ bool DataFlash_Block::check_wrapped(void)
return 1; return 1;
} }
// This funciton finds the last log number // This funciton finds the last log number
uint16_t DataFlash_Block::find_last_log(void) uint16_t DataFlash_Block::find_last_log(void)
{ {
@ -421,7 +417,6 @@ void DataFlash_Class::_print_log_entry(uint8_t msg_type,
port->println(); port->println();
} }
/* /*
print FMT specifiers for log dumps where we have wrapped in the print FMT specifiers for log dumps where we have wrapped in the
dataflash and so have no formats. This assumes the log being dumped dataflash and so have no formats. This assumes the log being dumped
@ -458,35 +453,35 @@ void DataFlash_Block::LogReadProcess(uint16_t log_num,
StartRead(start_page); StartRead(start_page);
while (true) { while (true) {
uint8_t data; uint8_t data;
ReadBlock(&data, 1); ReadBlock(&data, 1);
// This is a state machine to read the packets // This is a state machine to read the packets
switch(log_step) { switch(log_step) {
case 0: case 0:
if (data == HEAD_BYTE1) { if (data == HEAD_BYTE1) {
log_step++; log_step++;
} }
break; break;
case 1: case 1:
if (data == HEAD_BYTE2) { if (data == HEAD_BYTE2) {
log_step++; log_step++;
} else { } else {
log_step = 0; log_step = 0;
} }
break; break;
case 2: case 2:
log_step = 0; log_step = 0;
if (first_entry && data != LOG_FORMAT_MSG) { if (first_entry && data != LOG_FORMAT_MSG) {
_print_log_formats(port); _print_log_formats(port);
} }
first_entry = false; first_entry = false;
_print_log_entry(data, print_mode, port); _print_log_entry(data, print_mode, port);
break; break;
} }
uint16_t new_page = GetPage(); uint16_t new_page = GetPage();
if (new_page != page) { if (new_page != page) {
if (new_page == end_page+1 || new_page == start_page) { if (new_page == end_page+1 || new_page == start_page) {
@ -494,7 +489,7 @@ void DataFlash_Block::LogReadProcess(uint16_t log_num,
} }
page = new_page; page = new_page;
} }
} }
} }
/* /*
@ -619,7 +614,6 @@ void DataFlash_Class::Log_Write_Format(const struct LogStructure *s)
WriteBlock(&pkt, sizeof(pkt)); WriteBlock(&pkt, sizeof(pkt));
} }
/* /*
write a parameter to the log write a parameter to the log
*/ */
@ -666,8 +660,6 @@ void DataFlash_Class::Log_Write_Parameters(void)
} }
} }
// Write an GPS packet // Write an GPS packet
void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i, int32_t relative_alt) void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i, int32_t relative_alt)
{ {
@ -772,7 +764,7 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro)
LOG_PACKET_HEADER_INIT(LOG_BARO_MSG), LOG_PACKET_HEADER_INIT(LOG_BARO_MSG),
timestamp : now, timestamp : now,
altitude : baro.get_altitude(0), altitude : baro.get_altitude(0),
pressure : baro.get_pressure(0), pressure : baro.get_pressure(0),
temperature : (int16_t)(baro.get_temperature(0) * 100), temperature : (int16_t)(baro.get_temperature(0) * 100),
climbrate : baro.get_climb_rate() climbrate : baro.get_climb_rate()
}; };
@ -941,7 +933,7 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
}; };
WriteBlock(&pkt, sizeof(pkt)); WriteBlock(&pkt, sizeof(pkt));
// Write second EKF packet // Write second EKF packet
float ratio; float ratio;
float az1bias, az2bias; float az1bias, az2bias;
Vector3f wind; Vector3f wind;
@ -969,11 +961,11 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
}; };
WriteBlock(&pkt2, sizeof(pkt2)); WriteBlock(&pkt2, sizeof(pkt2));
// Write third EKF packet // Write third EKF packet
Vector3f velInnov; Vector3f velInnov;
Vector3f posInnov; Vector3f posInnov;
Vector3f magInnov; Vector3f magInnov;
float tasInnov; float tasInnov;
ahrs.get_NavEKF().getInnovations(velInnov, posInnov, magInnov, tasInnov); ahrs.get_NavEKF().getInnovations(velInnov, posInnov, magInnov, tasInnov);
struct log_EKF3 pkt3 = { struct log_EKF3 pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_EKF3_MSG), LOG_PACKET_HEADER_INIT(LOG_EKF3_MSG),
@ -991,12 +983,12 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
}; };
WriteBlock(&pkt3, sizeof(pkt3)); WriteBlock(&pkt3, sizeof(pkt3));
// Write fourth EKF packet // Write fourth EKF packet
float velVar; float velVar;
float posVar; float posVar;
float hgtVar; float hgtVar;
Vector3f magVar; Vector3f magVar;
float tasVar; float tasVar;
Vector2f offset; Vector2f offset;
uint8_t faultStatus, timeoutStatus; uint8_t faultStatus, timeoutStatus;
nav_filter_status solutionStatus; nav_filter_status solutionStatus;

Loading…
Cancel
Save