From 7e95edbbe848ec49ee81dbd85dc8c91558a83aa8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 May 2013 19:02:16 +0400 Subject: [PATCH] New messages added to sdlog2 --- src/modules/sdlog2/sdlog2.c | 189 +++++++++++++++------------ src/modules/sdlog2/sdlog2_format.h | 2 +- src/modules/sdlog2/sdlog2_messages.h | 77 +++++++++-- 3 files changed, 172 insertions(+), 96 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8d4d2f8ced..48d107945b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -81,6 +81,7 @@ #include #include "sdlog2_ringbuffer.h" +#include "sdlog2_format.h" #include "sdlog2_messages.h" static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -447,7 +448,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 25; + const ssize_t fdsc = 13; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -455,7 +456,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { - struct sensor_combined_s raw; + struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; struct actuator_outputs_s act_outputs; @@ -478,9 +479,9 @@ int sdlog2_thread_main(int argc, char *argv[]) int sensor_sub; int att_sub; int att_sp_sub; - int act_0_sub; - int controls_0_sub; - int controls_effective_0_sub; + int act_outputs_sub; + int act_controls_sub; + int act_controls_effective_sub; int local_pos_sub; int global_pos_sub; int gps_pos_sub; @@ -496,9 +497,13 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { LOG_PACKET_HEADER; union { - struct log_TS_s log_TS; + struct log_TIME_s log_TIME; struct log_ATT_s log_ATT; - struct log_RAW_s log_RAW; + struct log_ATSP_s log_ATSP; + struct log_IMU_s log_IMU; + struct log_SENS_s log_SENS; + struct log_LPOS_s log_LPOS; + struct log_LPSP_s log_LPSP; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -514,14 +519,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- GPS POSITION --- */ - /* subscribe to ORB for global position */ subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); fds[fdsc_count].fd = subs.gps_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- SENSORS RAW VALUE --- */ - /* subscribe to ORB for sensors raw */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; /* do not rate limit, instead use skip counter (aliasing on rate limit) */ @@ -529,89 +532,65 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- ATTITUDE VALUE --- */ - /* subscribe to ORB for attitude */ subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); fds[fdsc_count].fd = subs.att_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ATTITUDE SETPOINT VALUE --- */ - /* subscribe to ORB for attitude setpoint */ - /* struct already allocated */ subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); fds[fdsc_count].fd = subs.att_sp_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; - /** --- ACTUATOR OUTPUTS --- */ - subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - fds[fdsc_count].fd = subs.act_0_sub; + /* --- ACTUATOR OUTPUTS --- */ + subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + fds[fdsc_count].fd = subs.act_outputs_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ACTUATOR CONTROL VALUE --- */ - /* subscribe to ORB for actuator control */ - subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs.controls_0_sub; + subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + fds[fdsc_count].fd = subs.act_controls_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ - /* subscribe to ORB for actuator control */ - subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); - fds[fdsc_count].fd = subs.controls_effective_0_sub; + subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + fds[fdsc_count].fd = subs.act_controls_effective_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- LOCAL POSITION --- */ - /* subscribe to ORB for local position */ subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); fds[fdsc_count].fd = subs.local_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GLOBAL POSITION --- */ - /* subscribe to ORB for global position */ subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); fds[fdsc_count].fd = subs.global_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- VICON POSITION --- */ - /* subscribe to ORB for vicon position */ subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); fds[fdsc_count].fd = subs.vicon_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- FLOW measurements --- */ - /* subscribe to ORB for flow measurements */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- BATTERY STATUS --- */ - /* subscribe to ORB for flow measurements */ subs.batt_sub = orb_subscribe(ORB_ID(battery_status)); fds[fdsc_count].fd = subs.batt_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- DIFFERENTIAL PRESSURE --- */ - /* subscribe to ORB for flow measurements */ - subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - fds[fdsc_count].fd = subs.diff_pres_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- AIRSPEED --- */ - /* subscribe to ORB for airspeed */ - subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - fds[fdsc_count].fd = subs.airspeed_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. @@ -625,7 +604,7 @@ int sdlog2_thread_main(int argc, char *argv[]) * set up poll to block for new data, * wait for a maximum of 1000 ms (1 second) */ - // const int timeout = 1000; + const int poll_timeout = 1000; thread_running = true; @@ -641,13 +620,17 @@ int sdlog2_thread_main(int argc, char *argv[]) starttime = hrt_absolute_time(); - /* track skipping */ - int skip_count = 0; + /* track changes in sensor_combined topic */ + uint16_t gyro_counter = 0; + uint16_t accelerometer_counter = 0; + uint16_t magnetometer_counter = 0; + uint16_t baro_counter = 0; + uint16_t differential_pressure_counter = 0; while (!thread_should_exit) { /* poll all topics */ - int poll_ret = poll(fds, 4, 1000); + int poll_ret = poll(fds, fdsc, poll_timeout); /* handle the poll result */ if (poll_ret == 0) { @@ -666,11 +649,11 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_mutex_lock(&logbuffer_mutex); /* write time stamp message */ - log_msg.msg_type = LOG_TS_MSG; - log_msg.body.log_TS.t = hrt_absolute_time(); - sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(TS)); + log_msg.msg_type = LOG_TIME_MSG; + log_msg.body.log_TIME.t = hrt_absolute_time(); + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(TIME)); - /* --- VEHICLE COMMAND VALUE --- */ + /* --- VEHICLE COMMAND --- */ if (fds[ifds++].revents & POLLIN) { /* copy command into local buffer */ orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); @@ -680,19 +663,58 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + // TODO not implemented yet } - /* --- SENSORS RAW VALUE --- */ + /* --- SENSOR COMBINED --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - log_msg.msg_type = LOG_RAW_MSG; - log_msg.body.log_RAW.accX = buf.raw.accelerometer_m_s2[0]; - log_msg.body.log_RAW.accY = buf.raw.accelerometer_m_s2[1]; - log_msg.body.log_RAW.accZ = buf.raw.accelerometer_m_s2[2]; - sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(RAW)); + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); + bool write_IMU = false; + bool write_SENS = false; + if (buf.sensor.gyro_counter != gyro_counter) { + gyro_counter = buf.sensor.gyro_counter; + write_IMU = true; + } + if (buf.sensor.accelerometer_counter != accelerometer_counter) { + accelerometer_counter = buf.sensor.accelerometer_counter; + write_IMU = true; + } + if (buf.sensor.magnetometer_counter != magnetometer_counter) { + magnetometer_counter = buf.sensor.magnetometer_counter; + write_IMU = true; + } + if (buf.sensor.baro_counter != baro_counter) { + baro_counter = buf.sensor.baro_counter; + write_SENS = true; + } + if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { + differential_pressure_counter = buf.sensor.differential_pressure_counter; + write_SENS = true; + } + if (write_IMU) { + log_msg.msg_type = LOG_IMU_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(IMU)); + } + if (write_SENS) { + log_msg.msg_type = LOG_SENS_MSG; + log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(SENS)); + } } - /* --- ATTITUDE VALUE --- */ + /* --- ATTITUDE --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); log_msg.msg_type = LOG_ATT_MSG; @@ -702,69 +724,72 @@ int sdlog2_thread_main(int argc, char *argv[]) sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(ATT)); } - /* --- ATTITUDE SETPOINT VALUE --- */ + /* --- ATTITUDE SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp); - // TODO not implemented yet + log_msg.msg_type = LOG_ATSP_MSG; + log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; + log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; + log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(ATSP)); } /* --- ACTUATOR OUTPUTS --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs); // TODO not implemented yet } - /* --- ACTUATOR CONTROL VALUE --- */ + /* --- ACTUATOR CONTROL --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls); // TODO not implemented yet } - /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ + /* --- ACTUATOR CONTROL EFFECTIVE --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective); // TODO not implemented yet } /* --- LOCAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - // TODO not implemented yet + orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); + log_msg.msg_type = LOG_LPOS_MSG; + log_msg.body.log_LPOS.x = buf.local_pos.x; + log_msg.body.log_LPOS.y = buf.local_pos.y; + log_msg.body.log_LPOS.z = buf.local_pos.z; + log_msg.body.log_LPOS.vx = buf.local_pos.vx; + log_msg.body.log_LPOS.vy = buf.local_pos.vy; + log_msg.body.log_LPOS.vz = buf.local_pos.vz; + log_msg.body.log_LPOS.hdg = buf.local_pos.hdg; + log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat; + log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon; + log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt; + sdlog2_logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(LPOS)); } /* --- GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); // TODO not implemented yet } /* --- VICON POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); // TODO not implemented yet } - /* --- FLOW measurements --- */ + /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); // TODO not implemented yet } /* --- BATTERY STATUS --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - // TODO not implemented yet - } - - /* --- DIFFERENTIAL PRESSURE --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - // TODO not implemented yet - } - - /* --- AIRSPEED --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); // TODO not implemented yet } diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index bbf8b6f12f..59b91d90db 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -93,6 +93,6 @@ struct log_format_s { #define LOG_FORMAT_MSG 0x80 -#define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(log_msg.body.log_##_name) +#define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s) #endif /* SDLOG2_FORMAT_H_ */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index ae98ea0381..0bc999e243 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -47,13 +47,13 @@ /* define message formats */ -/* === 1: TS - TIME STAMP === */ -#define LOG_TS_MSG 1 -struct log_TS_s { +/* --- TIME - TIME STAMP --- */ +#define LOG_TIME_MSG 1 +struct log_TIME_s { uint64_t t; }; -/* === 2: ATT - ATTITUDE === */ +/* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 struct log_ATT_s { float roll; @@ -61,20 +61,71 @@ struct log_ATT_s { float yaw; }; -/* === 3: RAW - SENSORS === */ -#define LOG_RAW_MSG 3 -struct log_RAW_s { - float accX; - float accY; - float accZ; +/* --- ATSP - ATTITUDE SET POINT --- */ +#define LOG_ATSP_MSG 3 +struct log_ATSP_s { + float roll_sp; + float pitch_sp; + float yaw_sp; +}; + +/* --- IMU - IMU SENSORS --- */ +#define LOG_IMU_MSG 4 +struct log_IMU_s { + float acc_x; + float acc_y; + float acc_z; + float gyro_x; + float gyro_y; + float gyro_z; + float mag_x; + float mag_y; + float mag_z; +}; + +/* --- SENS - OTHER SENSORS --- */ +#define LOG_SENS_MSG 5 +struct log_SENS_s { + float baro_pres; + float baro_alt; + float baro_temp; + float diff_pres; +}; + +/* --- LPOS - LOCAL POSITION --- */ +#define LOG_LPOS_MSG 6 +struct log_LPOS_s { + float x; + float y; + float z; + float vx; + float vy; + float vz; + float hdg; + int32_t home_lat; + int32_t home_lon; + float home_alt; +}; + +/* --- LPOS - LOCAL POSITION SETPOINT --- */ +#define LOG_LPSP_MSG 7 +struct log_LPSP_s { + float x; + float y; + float z; + float yaw; }; /* construct list of all message formats */ static const struct log_format_s log_formats[] = { - LOG_FORMAT(TS, "Q", "t"), - LOG_FORMAT(ATT, "fff", "roll,pitch,yaw"), - LOG_FORMAT(RAW, "fff", "accX,accY,accZ"), + LOG_FORMAT(TIME, "Q", "t"), + LOG_FORMAT(ATT, "fff", "Roll,Pitch,Yaw"), + LOG_FORMAT(ATSP, "fff", "RollSP,PitchSP,YawSP"), + LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), + LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), + LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);