diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 94f7936118..76667ede97 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -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 diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 70d5cdf58e..1353d34832 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -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, diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index b6b5a6d748..3dbe583723 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -149,7 +149,7 @@ static void init_ardupilot() gcs0.reset_cli_timeout(); } if (g.log_bitmask != 0) { - DataFlash.start_new_log(); + start_logging(); } #endif