Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: convert to new logging system

This commit is contained in:
Andrew Tridgell 2013-04-19 23:37:21 +10:00
parent 8c7a7928d4
commit c56f338fe0
3 changed files with 46 additions and 259 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

299
ArduPlane/Log.pde
View File

@ -84,13 +84,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
}
DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
cliSerial->printf_P(PSTR("Dumping Log %u, start pg %u, end pg %u\n"),
(unsigned)dump_log,
(unsigned)dump_log_start,
(unsigned)dump_log_end);
Log_Read((uint16_t)dump_log, dump_log_start, dump_log_end);
cliSerial->printf_P(PSTR("Done\n"));
return 0;
}
@ -160,27 +154,7 @@ process_logs(uint8_t argc, const Menu::arg *argv)
return 0;
}
// print_latlon - prints an latitude or longitude value held in an int32_t
// probably this should be moved to AP_Common
void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon)
{
int32_t dec_portion, frac_portion;
int32_t abs_lat_or_lon = labs(lat_or_lon);
// extract decimal portion (special handling of negative numbers to ensure we round towards zero)
dec_portion = abs_lat_or_lon / T7;
// extract fractional portion
frac_portion = abs_lat_or_lon - dec_portion*T7;
// print output including the minus sign
if( lat_or_lon < 0 ) {
s->printf_P(PSTR("-"));
}
s->printf_P(PSTR("%ld.%07ld"),(long)dec_portion,(long)frac_portion);
}
struct log_Attitute {
struct PACKED log_Attitude {
LOG_PACKET_HEADER;
int32_t roll;
int32_t pitch;
@ -190,7 +164,7 @@ struct log_Attitute {
// Write an attitude packet. Total length : 10 bytes
static void Log_Write_Attitude(void)
{
struct log_Attitute pkt = {
struct log_Attitude pkt = {
LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
roll : ahrs.roll_sensor,
pitch : ahrs.pitch_sensor,
@ -199,18 +173,7 @@ static void Log_Write_Attitude(void)
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Read an attitude packet
static void Log_Read_Attitude()
{
struct log_Attitute pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("ATT, %ld, %ld, %ld\n"),
(long)pkt.roll,
(long)pkt.pitch,
(long)pkt.yaw);
}
struct log_Performance {
struct PACKED log_Performance {
LOG_PACKET_HEADER;
uint32_t loop_time;
uint16_t main_loop_count;
@ -243,26 +206,7 @@ static void Log_Write_Performance()
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Read a performance packet
static void Log_Read_Performance()
{
struct log_Performance pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("PM, %lu, %u, %d, %u, %u, %u, %d, %d, %d, %d\n"),
pkt.loop_time,
(unsigned)pkt.main_loop_count,
(int)pkt.g_dt_max,
(unsigned)pkt.renorm_count,
(unsigned)pkt.renorm_blowup,
(unsigned)pkt.gps_fix_count,
(int)pkt.gyro_drift_x,
(int)pkt.gyro_drift_y,
(int)pkt.gyro_drift_z,
(int)pkt.pm_test);
}
struct log_Cmd {
struct PACKED log_Cmd {
LOG_PACKET_HEADER;
uint8_t command_total;
uint8_t command_number;
@ -291,24 +235,7 @@ static void Log_Write_Cmd(uint8_t num, const struct Location *wp)
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Read a command processing packet
static void Log_Read_Cmd()
{
struct log_Cmd pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("CMD, %u, %u, %u, %u, %u, %ld, %ld, %ld\n"),
(unsigned)pkt.command_total,
(unsigned)pkt.command_number,
(unsigned)pkt.waypoint_id,
(unsigned)pkt.waypoint_options,
(unsigned)pkt.waypoint_param1,
(long)pkt.waypoint_altitude,
(long)pkt.waypoint_latitude,
(long)pkt.waypoint_longitude);
}
struct log_Camera {
struct PACKED log_Camera {
LOG_PACKET_HEADER;
uint32_t gps_time;
int32_t latitude;
@ -337,25 +264,7 @@ static void Log_Write_Camera()
#endif
}
// Read a camera packet
static void Log_Read_Camera()
{
struct log_Camera pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
// 1
cliSerial->printf_P(PSTR("CAMERA, %lu, "),(unsigned long)pkt.gps_time); // 1 time
print_latlon(cliSerial, pkt.latitude); // 2 lat
cliSerial->print_P(PSTR(", "));
print_latlon(cliSerial, pkt.longitude); // 3 lon
// 4 5 6 7
cliSerial->printf_P(PSTR(", %ld, %d, %d, %u\n"),
(long)pkt.altitude, // 4 altitude
(int)pkt.roll, // 5 roll in centidegrees
(int)pkt.pitch, // 6 pitch in centidegrees
(unsigned)pkt.yaw); // 7 yaw in centidegrees
}
struct log_Startup {
struct PACKED log_Startup {
LOG_PACKET_HEADER;
uint8_t startup_type;
uint8_t command_total;
@ -378,37 +287,15 @@ static void Log_Write_Startup(uint8_t type)
}
}
static void Log_Read_Startup()
{
struct log_Startup pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
switch( pkt.startup_type ) {
case TYPE_AIRSTART_MSG:
cliSerial->printf_P(PSTR("AIR START"));
break;
case TYPE_GROUNDSTART_MSG:
cliSerial->printf_P(PSTR("GROUND START"));
break;
default:
cliSerial->printf_P(PSTR("UNKNOWN STARTUP"));
break;
}
cliSerial->printf_P(PSTR(" - %u commands in memory\n"),(unsigned)pkt.command_total);
}
struct log_Control_Tuning {
struct PACKED log_Control_Tuning {
LOG_PACKET_HEADER;
int16_t roll_out;
int16_t nav_roll_cd;
int16_t roll;
int16_t pitch_out;
int16_t nav_pitch_cd;
int16_t pitch;
int16_t throttle_out;
int16_t rudder_out;
int16_t accel_y;
float accel_y;
};
// Write a control tuning packet. Total length : 22 bytes
@ -416,40 +303,19 @@ static void Log_Write_Control_Tuning()
{
Vector3f accel = ins.get_accel();
struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
roll_out : (int16_t)g.channel_roll.servo_out,
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
nav_roll_cd : (int16_t)nav_roll_cd,
roll : (int16_t)ahrs.roll_sensor,
pitch_out : (int16_t)g.channel_pitch.servo_out,
nav_pitch_cd : (int16_t)nav_pitch_cd,
pitch : (int16_t)ahrs.pitch_sensor,
throttle_out : (int16_t)g.channel_throttle.servo_out,
rudder_out : (int16_t)g.channel_rudder.servo_out,
accel_y : (int16_t)(accel.y * 10000)
accel_y : accel.y
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Read an control tuning packet
static void Log_Read_Control_Tuning()
{
struct log_Control_Tuning pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("CTUN, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f, %4.4f\n"),
(float)pkt.roll_out / 100.f,
(float)pkt.nav_roll_cd / 100.f,
(float)pkt.roll / 100.f,
(float)pkt.pitch_out / 100.f,
(float)pkt.nav_pitch_cd / 100.f,
(float)pkt.pitch / 100.f,
(float)pkt.throttle_out / 100.f,
(float)pkt.rudder_out / 100.f,
(float)pkt.accel_y / 10000.f
);
}
struct log_Nav_Tuning {
struct PACKED log_Nav_Tuning {
LOG_PACKET_HEADER;
uint16_t yaw;
uint32_t wp_distance;
@ -463,7 +329,7 @@ struct log_Nav_Tuning {
static void Log_Write_Nav_Tuning()
{
struct log_Nav_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_NAV_TUNING_MSG),
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
yaw : (uint16_t)ahrs.yaw_sensor,
wp_distance : wp_distance,
target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(),
@ -474,22 +340,7 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Read a nav tuning packet
static void Log_Read_Nav_Tuning()
{
struct log_Nav_Tuning pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("NTUN, %4.4f, %lu, %4.4f, %4.4f, %4.4f, %4.4f\n"),
(float)pkt.yaw/100.0f,
(unsigned long)pkt.wp_distance,
(float)(pkt.target_bearing_cd/100.0f),
(float)(pkt.nav_bearing_cd/100.0f),
(float)(pkt.altitude_error_cm/100.0f),
(float)(pkt.airspeed_cm/100.0f));
}
struct log_Mode {
struct PACKED log_Mode {
LOG_PACKET_HEADER;
uint8_t mode;
};
@ -504,53 +355,12 @@ static void Log_Write_Mode(uint8_t mode)
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Read a mode packet
static void Log_Read_Mode()
{
struct log_Mode pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("MOD,"));
print_flight_mode(pkt.mode);
}
// Read a GPS packet
static void Log_Read_GPS()
{
struct log_GPS pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("GPS, %ld, %u, "),
(long)pkt.gps_time,
(unsigned)pkt.num_sats);
print_latlon(cliSerial, pkt.latitude);
cliSerial->print_P(PSTR(", "));
print_latlon(cliSerial, pkt.longitude);
cliSerial->printf_P(PSTR(", %4.4f, %4.4f, %lu, %ld\n"),
(float)pkt.rel_altitude*0.01,
(float)pkt.altitude*0.01,
(unsigned long)pkt.ground_speed,
(long)pkt.ground_course);
}
// Read a raw accel/gyro packet
static void Log_Read_IMU()
{
struct log_IMU pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("IMU, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
pkt.gyro_x,
pkt.gyro_y,
pkt.gyro_z,
pkt.accel_x,
pkt.accel_y,
pkt.accel_z);
}
struct log_Current {
struct PACKED log_Current {
LOG_PACKET_HEADER;
int16_t throttle_in;
int16_t battery_voltage;
int16_t current_amps;
int16_t current_total;
float current_total;
};
static void Log_Write_Current()
@ -560,22 +370,32 @@ static void Log_Write_Current()
throttle_in : g.channel_throttle.control_in,
battery_voltage : (int16_t)(battery_voltage1 * 100.0),
current_amps : (int16_t)(current_amps1 * 100.0),
current_total : (int16_t)current_total1
current_total : current_total1
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Read a Current packet
static void Log_Read_Current()
{
struct log_Current pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
cliSerial->printf_P(PSTR("CURRENT, %d, %4.4f, %4.4f, %d\n"),
(int)pkt.throttle_in,
((float)pkt.battery_voltage / 100.f),
((float)pkt.current_amps / 100.f),
(int)pkt.current_total);
}
static const struct LogStructure log_structure[] PROGMEM = {
LOG_COMMON_STRUCTURES,
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),
"ATT", "ccC", "Roll,Pitch,Yaw" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "IHhBBBhhhh", "LTime,MLC,gDt,RNCnt,RNBl,GPScnt,GDx,GDy,GDz,PMT" },
{ LOG_CMD_MSG, sizeof(log_Cmd),
"CMD", "BBBBBeLL", "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
{ LOG_CAMERA_MSG, sizeof(log_Camera),
"CAM", "ILLeccC", "GPSTime,Lat,Lng,Alt,Roll,Pitch,Yaw" },
{ LOG_STARTUP_MSG, sizeof(log_Startup),
"STRT", "BB", "SType,CTot" },
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
"CTUN", "cccchhf", "NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,AccY" },
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
"NTUN", "CICCcc", "Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd" },
{ LOG_MODE_MSG, sizeof(log_Mode),
"MODE", "B", "Mode" },
{ LOG_CURRENT_MSG, sizeof(log_Current),
"CURR", "hhhf", "Thr,Volt,Curr,CurrTot" },
};
// Read the DataFlash.log memory : Packet Parser
static void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
@ -586,50 +406,17 @@ static void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
cliSerial->println_P(PSTR(HAL_BOARD_NAME));
DataFlash.log_read_process(log_num, start_page, end_page, log_callback);
DataFlash.LogReadProcess(log_num, start_page, end_page,
sizeof(log_structure)/sizeof(log_structure[0]),
log_structure, cliSerial);
}
// Read the DataFlash.log memory : Packet Parser
static void log_callback(uint8_t msgid)
// start a new log
static void start_logging()
{
switch (msgid) {
case LOG_ATTITUDE_MSG:
Log_Read_Attitude();
break;
case LOG_MODE_MSG:
Log_Read_Mode();
break;
case LOG_CONTROL_TUNING_MSG:
Log_Read_Control_Tuning();
break;
case LOG_NAV_TUNING_MSG:
Log_Read_Nav_Tuning();
break;
case LOG_PERFORMANCE_MSG:
Log_Read_Performance();
break;
case LOG_IMU_MSG:
Log_Read_IMU();
break;
case LOG_CMD_MSG:
Log_Read_Cmd();
break;
case LOG_CURRENT_MSG:
Log_Read_Current();
break;
case LOG_STARTUP_MSG:
Log_Read_Startup();
break;
case LOG_GPS_MSG:
Log_Read_GPS();
break;
case LOG_CAMERA_MSG:
Log_Read_Camera();
break;
}
DataFlash.StartNewLog(sizeof(log_structure)/sizeof(log_structure[0]), log_structure);
}
#else // LOGGING_ENABLED
// dummy functions

4
ArduPlane/defines.h
View File

@ -158,8 +158,8 @@ enum gcs_severity {
// mark unused ones as 'deprecated', but leave them in
enum log_messages {
LOG_INDEX_MSG,
LOG_CONTROL_TUNING_MSG,
LOG_NAV_TUNING_MSG,
LOG_CTUN_MSG,
LOG_NTUN_MSG,
LOG_PERFORMANCE_MSG,
LOG_CMD_MSG,
LOG_CURRENT_MSG,

2
ArduPlane/system.pde
View File

@ -149,7 +149,7 @@ static void init_ardupilot()
gcs0.reset_cli_timeout();
}
if (g.log_bitmask != 0) {
DataFlash.start_new_log();
start_logging();
}
#endif