diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-08-16 13:23:33 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-08-16 13:23:33 +0200 |
commit | f78666e0c8afd1fc8ac74f4cf2acc79ebe8b8476 (patch) | |
tree | 100ac05749e5c899d264de1d129d6d98935b2491 /src/modules/sdlog2 | |
parent | 3f9f1d60e03f501209deb6c7532c37dfb786f343 (diff) | |
parent | a013fc5d0b28cd72f41a28c8229c2d7ab99965f4 (diff) | |
download | px4-firmware-f78666e0c8afd1fc8ac74f4cf2acc79ebe8b8476.tar.gz px4-firmware-f78666e0c8afd1fc8ac74f4cf2acc79ebe8b8476.tar.bz2 px4-firmware-f78666e0c8afd1fc8ac74f4cf2acc79ebe8b8476.zip |
Merge branch 'seatbelt_multirotor' into seatbelt_multirotor_new
WIP, TODO fixedwing
Diffstat (limited to 'src/modules/sdlog2')
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 169 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 54 |
2 files changed, 65 insertions, 158 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index fefa539f9..30dc7df9e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -60,7 +60,6 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> -#include <uORB/topics/actuator_armed.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> @@ -76,7 +75,7 @@ #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/vehicle_control_debug.h> +#include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/differential_pressure.h> @@ -96,7 +95,6 @@ log_msgs_written++; \ } else { \ log_msgs_skipped++; \ - /*printf("skip\n");*/ \ } #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ @@ -104,9 +102,6 @@ fds[fdsc_count].events = POLLIN; \ fdsc_count++; - -//#define SDLOG2_DEBUG - static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -194,7 +189,7 @@ static int file_copy(const char *file_old, const char *file_new); static void handle_command(struct vehicle_command_s *cmd); -static void handle_status(struct actuator_armed_s *armed); +static void handle_status(struct vehicle_status_s *cmd); /** * Create folder for current logging session. Store folder name in 'log_folder'. @@ -235,7 +230,7 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("sdlog2 already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } @@ -252,7 +247,7 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (!thread_running) { - printf("\tsdlog2 is not started\n"); + warnx("not started"); } main_thread_should_exit = true; @@ -264,7 +259,7 @@ int sdlog2_main(int argc, char *argv[]) sdlog2_status(); } else { - printf("\tsdlog2 not started\n"); + warnx("not started\n"); } exit(0); @@ -389,11 +384,6 @@ static void *logwriter_thread(void *arg) /* only get pointer to thread-safe data, do heavy I/O a few lines down */ int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part); -#ifdef SDLOG2_DEBUG - int rp = logbuf->read_ptr; - int wp = logbuf->write_ptr; -#endif - /* continue */ pthread_mutex_unlock(&logbuffer_mutex); @@ -409,9 +399,6 @@ static void *logwriter_thread(void *arg) n = write(log_file, read_ptr, n); should_wait = (n == available) && !is_part; -#ifdef SDLOG2_DEBUG - printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait); -#endif if (n < 0) { main_thread_should_exit = true; @@ -424,14 +411,8 @@ static void *logwriter_thread(void *arg) } else { n = 0; -#ifdef SDLOG2_DEBUG - printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit); -#endif /* exit only with empty buffer */ if (main_thread_should_exit || logwriter_should_exit) { -#ifdef SDLOG2_DEBUG - printf("break logwriter thread\n"); -#endif break; } should_wait = true; @@ -446,10 +427,6 @@ static void *logwriter_thread(void *arg) fsync(log_file); close(log_file); -#ifdef SDLOG2_DEBUG - printf("logwriter thread exit\n"); -#endif - return OK; } @@ -606,15 +583,6 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "unable to create logging folder, exiting."); } - const char *converter_in = "/etc/logging/conv.zip"; - char* converter_out = malloc(150); - sprintf(converter_out, "%s/conv.zip", folder_path); - - if (file_copy(converter_in, converter_out)) { - errx(1, "unable to copy conversion scripts, exiting."); - } - free(converter_out); - /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); @@ -625,21 +593,9 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "can't allocate log buffer, exiting."); } - /* --- IMPORTANT: DEFINE MAX NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* max number of messages */ - const ssize_t fdsc = 21; - - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); - struct actuator_armed_s buf_armed; - memset(&buf_armed, 0, sizeof(buf_armed)); - /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; @@ -656,19 +612,18 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_setpoint_s global_pos_sp; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; - struct vehicle_control_debug_s control_debug; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; struct airspeed_s airspeed; struct esc_status_s esc; + struct vehicle_global_velocity_setpoint_s global_vel_sp; } buf; memset(&buf, 0, sizeof(buf)); struct { int cmd_sub; int status_sub; - int armed_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -682,11 +637,11 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sp_sub; int gps_pos_sub; int vicon_pos_sub; - int control_debug_sub; int flow_sub; int rc_sub; int airspeed_sub; int esc_sub; + int global_vel_sp_sub; } subs; /* log message buffer: header + body */ @@ -704,7 +659,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; - struct log_CTRL_s log_CTRL; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; struct log_AIRS_s log_AIRS; @@ -713,6 +667,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPOS_s log_GPOS; struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; + struct log_GVSP_s log_GVSP; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -720,18 +675,20 @@ int sdlog2_thread_main(int argc, char *argv[]) #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); + /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ + /* number of messages */ + const ssize_t fdsc = 20; + /* Sanity check variable and index */ + ssize_t fdsc_count = 0; + /* file descriptors to wait for */ + struct pollfd fds[fdsc]; + /* --- VEHICLE COMMAND --- */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); fds[fdsc_count].fd = subs.cmd_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- ACTUATOR ARMED --- */ - subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - fds[fdsc_count].fd = subs.armed_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- VEHICLE STATUS --- */ subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); fds[fdsc_count].fd = subs.status_sub; @@ -816,12 +773,6 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- CONTROL DEBUG --- */ - subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug)); - fds[fdsc_count].fd = subs.control_debug_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; @@ -846,6 +797,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- GLOBAL VELOCITY SETPOINT --- */ + subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); + fds[fdsc_count].fd = subs.global_vel_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -905,33 +862,22 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } - /* --- ARMED- LOG MANAGEMENT --- */ + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed); + orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); if (log_when_armed) { - handle_status(&buf_armed); + handle_status(&buf_status); } handled_topics++; } - /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); - - //if (log_when_armed) { - // handle_status(&buf_armed); - //} - - //handled_topics++; - } - if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } - ifds = 2; // Begin from fds[2] again + ifds = 1; // Begin from fds[1] again pthread_mutex_lock(&logbuffer_mutex); @@ -944,16 +890,9 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; - // XXX fix this - // log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; - // log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; - // log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; - // log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; - log_msg.body.log_STAT.state = 0; - log_msg.body.log_STAT.flight_mode = 0; - log_msg.body.log_STAT.manual_control_mode = 0; - log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.main_state = (unsigned char) buf_status.main_state; + log_msg.body.log_STAT.navigation_state = (unsigned char) buf_status.navigation_state; + log_msg.body.log_STAT.arming_state = (unsigned char) buf_status.arming_state; log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1044,9 +983,6 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; - log_msg.body.log_ATT.roll_acc = buf.att.rollacc; - log_msg.body.log_ATT.pitch_acc = buf.att.pitchacc; - log_msg.body.log_ATT.yaw_acc = buf.att.yawacc; LOGBUFFER_WRITE_AND_COUNT(ATT); } @@ -1162,27 +1098,6 @@ int sdlog2_thread_main(int argc, char *argv[]) // TODO not implemented yet } - /* --- CONTROL DEBUG --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); - - log_msg.msg_type = LOG_CTRL_MSG; - log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; - log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; - log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; - log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; - log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; - log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; - log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; - log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; - log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; - log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; - log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; - log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; - - LOGBUFFER_WRITE_AND_COUNT(CTRL); - } - /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); @@ -1237,14 +1152,18 @@ int sdlog2_thread_main(int argc, char *argv[]) } } -#ifdef SDLOG2_DEBUG - printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif + /* --- GLOBAL VELOCITY SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp); + log_msg.msg_type = LOG_GVSP_MSG; + log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; + log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; + log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; + LOGBUFFER_WRITE_AND_COUNT(GVSP); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { -#ifdef SDLOG2_DEBUG - printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif /* only request write if several packets can be written at once */ pthread_cond_signal(&logbuffer_cond); } @@ -1327,7 +1246,7 @@ int file_copy(const char *file_old, const char *file_new) fclose(source); fclose(target); - return OK; + return ret; } void handle_command(struct vehicle_command_s *cmd) @@ -1357,10 +1276,12 @@ void handle_command(struct vehicle_command_s *cmd) } } -void handle_status(struct actuator_armed_s *armed) +void handle_status(struct vehicle_status_s *status) { - if (armed->armed != flag_system_armed) { - flag_system_armed = armed->armed; + // TODO use flag from actuator_armed here? + bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR; + if (armed != flag_system_armed) { + flag_system_armed = armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9be96a62e..38e2596b2 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -63,9 +63,6 @@ struct log_ATT_s { float roll_rate; float pitch_rate; float yaw_rate; - float roll_acc; - float pitch_acc; - float yaw_acc; }; /* --- ATSP - ATTITUDE SET POINT --- */ @@ -152,55 +149,36 @@ struct log_ATTC_s { /* --- STAT - VEHICLE STATE --- */ #define LOG_STAT_MSG 10 struct log_STAT_s { - uint8_t state; - uint8_t flight_mode; - uint8_t manual_control_mode; - uint8_t manual_sas_mode; - uint8_t armed; + uint8_t main_state; + uint8_t navigation_state; + uint8_t arming_state; float battery_voltage; float battery_current; float battery_remaining; uint8_t battery_warning; }; -/* --- CTRL - CONTROL DEBUG --- */ -#define LOG_CTRL_MSG 11 -struct log_CTRL_s { - float roll_p; - float roll_i; - float roll_d; - float roll_rate_p; - float roll_rate_i; - float roll_rate_d; - float pitch_p; - float pitch_i; - float pitch_d; - float pitch_rate_p; - float pitch_rate_i; - float pitch_rate_d; -}; - /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 12 +#define LOG_RC_MSG 11 struct log_RC_s { float channel[8]; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 13 +#define LOG_OUT0_MSG 12 struct log_OUT0_s { float output[8]; }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 14 +#define LOG_AIRS_MSG 13 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 15 +#define LOG_ARSP_MSG 14 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; @@ -208,7 +186,7 @@ struct log_ARSP_s { }; /* --- FLOW - OPTICAL FLOW --- */ -#define LOG_FLOW_MSG 16 +#define LOG_FLOW_MSG 15 struct log_FLOW_s { int16_t flow_raw_x; int16_t flow_raw_y; @@ -264,13 +242,21 @@ struct log_ESC_s { uint16_t esc_setpoint_raw; }; +/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ +#define LOG_GVSP_MSG 19 +struct log_GVSP_s { + float vx; + float vy; + float vz; +}; + #pragma pack(pop) /* construct list of all message formats */ static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), - LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollR,PitchR,YawR,RollA,PitchA,YawA"), + LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), @@ -278,8 +264,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), - LOG_FORMAT(CTRL, "ffffffffffff", "RP,RI,RD,RRP,RRI,RRD,PP,PI,PD,PRP,PRI,PRD"), + LOG_FORMAT(STAT, "BBBfffB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), @@ -287,7 +272,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), - LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); |