diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 42 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 25 |
2 files changed, 60 insertions, 7 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b5821098f..32bd422bc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -597,10 +597,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* file descriptors to wait for */ struct pollfd fds[fdsc]; + struct vehicle_status_s buf_status; + memset(&buf_status, 0, sizeof(buf_status)); + /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; - struct vehicle_status_s status; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -652,6 +654,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; struct log_GPS_s log_GPS; + struct log_ATTC_s log_ATTC; + struct log_STAT_s log_STAT; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -802,22 +806,25 @@ int sdlog2_thread_main(int argc, char *argv[]) * logging_enabled can be changed while checking vehicle_command and vehicle_status */ bool check_data = logging_enabled; int ifds = 0; + int handled_topics = 0; - /* --- VEHICLE COMMAND --- */ + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); handle_command(&buf.cmd); + handled_topics++; } - /* --- VEHICLE STATUS --- */ + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf.status); + orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); if (log_when_armed) { - handle_status(&buf.status); + handle_status(&buf_status); } + handled_topics++; } - if (!logging_enabled || !check_data) { + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } @@ -828,6 +835,22 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TIME.t = hrt_absolute_time(); LOGBUFFER_WRITE_AND_COUNT(TIME); + /* --- VEHICLE STATUS --- */ + if (fds[ifds++].revents & POLLIN) { + // Don't orb_copy, it's already done few lines above + log_msg.msg_type = LOG_STAT_MSG; + 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.armed = (unsigned char) buf_status.flag_system_armed; + log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; + log_msg.body.log_STAT.battery_current = buf_status.current_battery; + log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; + log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; + LOGBUFFER_WRITE_AND_COUNT(STAT); + } + /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); @@ -930,7 +953,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ACTUATOR CONTROL --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls); - // TODO not implemented yet + log_msg.msg_type = LOG_ATTC_MSG; + log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; + log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; + log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; + log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; + LOGBUFFER_WRITE_AND_COUNT(ATTC); } /* --- ACTUATOR CONTROL EFFECTIVE --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 316459b1e..2b6b86521 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -132,6 +132,29 @@ struct log_GPS_s { float vel_d; float cog; }; + +/* --- ATTC - ATTITUDE CONTROLS --- */ +#define LOG_ATTC_MSG 9 +struct log_ATTC_s { + float roll; + float pitch; + float yaw; + float thrust; +}; + +/* --- STAT - VEHICLE STATE --- */ +#define LOG_STAT_MSG 10 +struct log_STAT_s { + unsigned char state; + unsigned char flight_mode; + unsigned char manual_control_mode; + unsigned char manual_sas_mode; + unsigned char armed; + float battery_voltage; + float battery_current; + float battery_remaining; + unsigned char battery_warning; +}; #pragma pack(pop) /* construct list of all message formats */ @@ -145,6 +168,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), 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"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); |