From 90f70707b1ad2c94acab045104fe98c508bcf6ed Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 19 Apr 2013 17:53:07 +1000 Subject: [PATCH] Rover: convert to new logging system --- APMrover2/APMrover2.pde | 14 +- APMrover2/GCS_Mavlink.pde | 7 +- APMrover2/Log.pde | 393 +++++++------------------------------- APMrover2/defines.h | 21 +- APMrover2/setup.pde | 2 +- APMrover2/system.pde | 7 +- 6 files changed, 94 insertions(+), 350 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 5797d577cb..e883a0eceb 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -125,11 +125,12 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...); // DataFlash //////////////////////////////////////////////////////////////////////////////// #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 -DataFlash_APM1 DataFlash; +static DataFlash_APM1 DataFlash; #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 -DataFlash_APM2 DataFlash; +static DataFlash_APM2 DataFlash; #elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL -DataFlash_SITL DataFlash; +//static DataFlash_File DataFlash("/tmp/APMlogs"); +static DataFlash_SITL DataFlash; #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 static DataFlash_File DataFlash("/fs/microsd/APM/logs"); #else @@ -641,7 +642,7 @@ static void fast_loop() Log_Write_Attitude(); if (g.log_bitmask & MASK_LOG_IMU) - Log_Write_IMU(); + DataFlash.Log_Write_IMU(&ins); #endif // custom code/exceptions for flight modes @@ -719,7 +720,7 @@ static void medium_loop() Log_Write_Nav_Tuning(); if (g.log_bitmask & MASK_LOG_GPS) - Log_Write_GPS(); + DataFlash.Log_Write_GPS(g_gps, current_loc.alt); break; // This case controls the slow loop @@ -779,10 +780,7 @@ static void slow_loop() mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter -#if USB_MUX_PIN > 0 check_usb_mux(); -#endif - break; } } diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 6b16d0edc1..4b9e6a78b6 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -1274,7 +1274,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) gcs_send_text_fmt(PSTR("Unknown parameter index %d"), packet.param_index); break; } - vp->copy_name_token(&token, param_name, AP_MAX_NAME_SIZE, true); + vp->copy_name_token(token, param_name, AP_MAX_NAME_SIZE, true); param_name[AP_MAX_NAME_SIZE] = 0; } else { strncpy(param_name, packet.param_id, AP_MAX_NAME_SIZE); @@ -1625,6 +1625,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mav_var_type(var_type), _count_parameters(), -1); // XXX we don't actually know what its index is... + DataFlash.Log_Write_Parameter(key, vp->cast_to_float(var_type)); } break; @@ -1817,7 +1818,7 @@ GCS_MAVLINK::queued_param_send() value = vp->cast_to_float(_queued_parameter_type); char param_name[AP_MAX_NAME_SIZE]; - vp->copy_name_token(&_queued_parameter_token, param_name, sizeof(param_name), true); + vp->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true); mavlink_msg_param_value_send( chan, @@ -1881,9 +1882,7 @@ static void mavlink_delay_cb() last_5s = tnow; gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM...")); } -#if USB_MUX_PIN > 0 check_usb_mux(); -#endif in_mavlink_delay = false; } diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index d9b0de5949..3852252d24 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -175,38 +175,7 @@ void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon) s->printf_P(PSTR("%ld.%07ld"),(long)dec_portion,(long)frac_portion); } - -struct log_Attitute { - LOG_PACKET_HEADER; - int16_t roll; - int16_t pitch; - uint16_t yaw; -}; - -// Write an attitude packet. Total length : 10 bytes -static void Log_Write_Attitude() -{ - struct log_Attitute pkt = { - LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), - roll : (int16_t)ahrs.roll_sensor, - pitch : (int16_t)ahrs.pitch_sensor, - yaw : (uint16_t)ahrs.yaw_sensor - }; - 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, %d, %d, %u\n"), - (int)pkt.roll, - (int)pkt.pitch, - (unsigned)pkt.yaw); -} - -struct log_Performance { +struct PACKED log_Performance { LOG_PACKET_HEADER; uint32_t loop_time; uint16_t main_loop_count; @@ -241,26 +210,7 @@ static void Log_Write_Performance() } #endif -// 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; @@ -289,24 +239,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_Startup { +struct PACKED log_Startup { LOG_PACKET_HEADER; uint8_t startup_type; uint8_t command_total; @@ -329,33 +262,13 @@ 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 steer_out; int16_t roll; int16_t pitch; int16_t throttle_out; - int16_t accel_y; + float accel_y; }; // Write a control tuning packet. Total length : 22 bytes @@ -364,33 +277,18 @@ static void Log_Write_Control_Tuning() { Vector3f accel = ins.get_accel(); struct log_Control_Tuning pkt = { - LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), + LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG), steer_out : (int16_t)g.channel_steer.servo_out, roll : (int16_t)ahrs.roll_sensor, pitch : (int16_t)ahrs.pitch_sensor, throttle_out : (int16_t)g.channel_throttle.servo_out, - accel_y : (int16_t)(accel.y * 10000) + accel_y : accel.y }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } #endif -// 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\n"), - (float)pkt.steer_out / 100.f, - (float)pkt.roll / 100.f, - (float)pkt.pitch / 100.f, - (float)pkt.throttle_out / 100.f, - (float)pkt.accel_y / 10000.f - ); -} - -struct log_Nav_Tuning { +struct PACKED log_Nav_Tuning { LOG_PACKET_HEADER; uint16_t yaw; float wp_distance; @@ -404,7 +302,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)target_bearing, @@ -415,22 +313,43 @@ 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)); +struct PACKED log_Attitude { + LOG_PACKET_HEADER; + int16_t roll; + int16_t pitch; + uint16_t yaw; +}; - cliSerial->printf_P(PSTR("NTUN, %4.4f, %4.2f, %4.4f, %4.4f, %4.4f, %d\n"), - (float)pkt.yaw/100.0f, - pkt.wp_distance, - (float)(pkt.target_bearing_cd/100.0f), - (float)(pkt.nav_bearing_cd/100.0f), - (float)(pkt.nav_gain_scalar/100.0f), - (int)pkt.throttle); + +// Write an attitude packet +void Log_Write_Attitude() +{ + struct log_Attitude pkt = { + LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), + roll : (int16_t)ahrs.roll_sensor, + pitch : (int16_t)ahrs.pitch_sensor, + yaw : (uint16_t)ahrs.yaw_sensor + }; + DataFlash.WriteBlock(&pkt, sizeof(pkt)); } -struct log_Sonar { +struct log_Mode { + LOG_PACKET_HEADER; + uint8_t mode; +}; + +// Write a mode packet. Total length : 7 bytes +static void Log_Write_Mode() +{ + struct log_Mode pkt = { + LOG_PACKET_HEADER_INIT(LOG_MODE_MSG), + mode : (uint8_t)control_mode + }; + DataFlash.WriteBlock(&pkt, sizeof(pkt)); +} + + +struct PACKED log_Sonar { LOG_PACKET_HEADER; int16_t nav_steer; uint16_t sonar1_distance; @@ -465,137 +384,12 @@ static void Log_Write_Sonar() } #endif -// Read a sonar packet -static void Log_Read_Sonar() -{ - struct log_Sonar pkt; - DataFlash.ReadPacket(&pkt, sizeof(pkt)); - - cliSerial->printf_P(PSTR("SONR, %d, %u, %u, %u, %d, %u, %u, %d\n"), - (int)pkt.nav_steer, - (unsigned)pkt.sonar1_distance, - (unsigned)pkt.sonar2_distance, - (unsigned)pkt.detected_count, - (int)pkt.turn_angle, - (unsigned)pkt.turn_time, - (unsigned)pkt.ground_speed, - (int)pkt.throttle); -} - - -struct log_Mode { - LOG_PACKET_HEADER; - uint8_t mode; -}; - -// Write a mode packet. Total length : 5 bytes -static void Log_Write_Mode(uint8_t mode) -{ - struct log_Mode pkt = { - LOG_PACKET_HEADER_INIT(LOG_MODE_MSG), - mode : 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_mode(pkt.mode); -} - -struct log_GPS { - LOG_PACKET_HEADER; - uint32_t gps_time; - uint8_t status; - uint8_t num_sats; - int32_t latitude; - int32_t longitude; - int32_t altitude; - uint32_t ground_speed; - int32_t ground_course; - int16_t hdop; -}; - -// Write an GPS packet. Total length : 30 bytes -static void Log_Write_GPS() -{ - struct log_GPS pkt = { - LOG_PACKET_HEADER_INIT(LOG_GPS_MSG), - gps_time : g_gps->time, - status : g_gps->status(), - num_sats : g_gps->num_sats, - latitude : current_loc.lat, - longitude : current_loc.lng, - altitude : g_gps->altitude, - ground_speed : g_gps->ground_speed, - ground_course : g_gps->ground_course, - hdop : g_gps->hdop - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} - -// 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, %u, "), - (long)pkt.gps_time, - (unsigned)pkt.status, - (unsigned)pkt.num_sats); - print_latlon(cliSerial, pkt.latitude); - cliSerial->print_P(PSTR(", ")); - print_latlon(cliSerial, pkt.longitude); - cliSerial->printf_P(PSTR(", %4.4f, %lu, %ld, %d\n"), - (float)pkt.altitude*0.01, - (unsigned long)pkt.ground_speed, - (long)pkt.ground_course, - (int)pkt.hdop); -} - -struct log_IMU { - LOG_PACKET_HEADER; - Vector3f gyro; - Vector3f accel; -}; - -// Write an raw accel/gyro data packet. Total length : 28 bytes -#if HIL_MODE != HIL_MODE_ATTITUDE -static void Log_Write_IMU() -{ - struct log_IMU pkt = { - LOG_PACKET_HEADER_INIT(LOG_IMU_MSG), - gyro : ins.get_gyro(), - accel : ins.get_accel() - }; - DataFlash.WriteBlock(&pkt, sizeof(pkt)); -} -#endif - -// 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() @@ -605,22 +399,33 @@ 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_STARTUP_MSG, sizeof(log_Startup), + "STRT", "BB", "SType,CTot" }, + { LOG_CTUN_MSG, sizeof(log_Control_Tuning), + "CTUN", "hcchf", "Steer,Roll,Pitch,ThrOut,AccY" }, + { LOG_NTUN_MSG, sizeof(log_Nav_Tuning), + "NTUN", "HfHHhb", "Yaw,WpDist,TargBrg,NavBrg,NavGain,Thr" }, + { LOG_SONAR_MSG, sizeof(log_Sonar), + "SONR", "hHHHbHCb", "NavStr,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" }, + { LOG_CURRENT_MSG, sizeof(log_Current), + "CURR", "hhhf", "Thr,Volt,Curr,CurrTot" }, + { LOG_MODE_MSG, sizeof(log_Mode), + "MODE", "B", "Mode" }, +}; + static int8_t setup_show (uint8_t argc, const Menu::arg *argv); @@ -633,77 +438,21 @@ static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) cliSerial->println_P(PSTR(HAL_BOARD_NAME)); -#if CLI_ENABLED == ENABLED - setup_show(0, NULL); -#endif - - DataFlash.log_read_process(log_num, start_page, end_page, log_callback); -} - -// Read the DataFlash log memory : Packet Parser -static void log_callback(uint8_t msgid) -{ - 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_SONAR_MSG: - Log_Read_Sonar(); - break; - } + DataFlash.LogReadProcess(log_num, start_page, end_page, + sizeof(log_structure)/sizeof(log_structure[0]), + log_structure, cliSerial); } #else // LOGGING_ENABLED // dummy functions -static void Log_Write_Mode(uint8_t mode) {} static void Log_Write_Startup(uint8_t type) {} static void Log_Write_Cmd(uint8_t num, const struct Location *wp) {} static void Log_Write_Current() {} static void Log_Write_Nav_Tuning() {} -static void Log_Write_GPS() {} static void Log_Write_Performance() {} static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } -static void Log_Write_Attitude() {} static void Log_Write_Control_Tuning() {} -static void Log_Write_IMU() {} static void Log_Write_Sonar() {} diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 9700aa6879..0a3f98139e 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -126,18 +126,15 @@ enum gcs_severity { }; // Logging parameters -#define LOG_INDEX_MSG 0xF0 -#define LOG_ATTITUDE_MSG 0x01 -#define LOG_GPS_MSG 0x02 -#define LOG_MODE_MSG 0x03 -#define LOG_CONTROL_TUNING_MSG 0x04 -#define LOG_NAV_TUNING_MSG 0x05 -#define LOG_PERFORMANCE_MSG 0x06 -#define LOG_IMU_MSG 0x07 -#define LOG_CMD_MSG 0x08 -#define LOG_CURRENT_MSG 0x09 -#define LOG_STARTUP_MSG 0x0A -#define LOG_SONAR_MSG 0x0B +#define LOG_CTUN_MSG 0x01 +#define LOG_NTUN_MSG 0x02 +#define LOG_PERFORMANCE_MSG 0x03 +#define LOG_CMD_MSG 0x04 +#define LOG_CURRENT_MSG 0x05 +#define LOG_STARTUP_MSG 0x06 +#define LOG_SONAR_MSG 0x07 +#define LOG_ATTITUDE_MSG 0x08 +#define LOG_MODE_MSG 0x09 #define TYPE_AIRSTART_MSG 0x00 #define TYPE_GROUNDSTART_MSG 0x01 diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index 84eb7d810a..21da38e1c9 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -117,7 +117,7 @@ setup_show(uint8_t argc, const Menu::arg *argv) cliSerial->printf_P(PSTR("Raw Values\n")); print_divider(); - AP_Param::show_all(); + AP_Param::show_all(cliSerial); return(0); } diff --git a/APMrover2/system.pde b/APMrover2/system.pde index bee5b90e29..8f76c6d31f 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -334,7 +334,7 @@ static void set_mode(enum mode mode) } if (g.log_bitmask & MASK_LOG_MODE) - Log_Write_Mode(control_mode); + Log_Write_Mode(); } /* @@ -472,9 +472,9 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) } -#if USB_MUX_PIN > 0 static void check_usb_mux(void) { +#if USB_MUX_PIN > 0 bool usb_check = !digitalRead(USB_MUX_PIN); if (usb_check == usb_connected) { return; @@ -487,8 +487,9 @@ static void check_usb_mux(void) } else { hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); } -} #endif +} + /*