diff options
Diffstat (limited to 'src/modules/sdlog2/sdlog2.c')
| -rw-r--r-- | src/modules/sdlog2/sdlog2.c | 45 |
1 files changed, 40 insertions, 5 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index af580f1f7..6df677338 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -496,6 +496,8 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); + perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write"); + int log_fd = open_log_file(); if (log_fd < 0) { @@ -553,7 +555,9 @@ static void *logwriter_thread(void *arg) n = available; } + perf_begin(perf_write); n = write(log_fd, read_ptr, n); + perf_end(perf_write); should_wait = (n == available) && !is_part; @@ -586,6 +590,9 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); + /* free performance counter */ + perf_free(perf_write); + return NULL; } @@ -934,6 +941,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; + struct actuator_controls_s act_controls1; struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; @@ -1015,6 +1023,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int rates_sp_sub; int act_outputs_sub; int act_controls_sub; + int act_controls_1_sub; int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; @@ -1048,6 +1057,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1)); subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -1368,6 +1378,18 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.gy = buf.att.g_comp[1]; log_msg.body.log_ATT.gz = buf.att.g_comp[2]; LOGBUFFER_WRITE_AND_COUNT(ATT); + // secondary attitude + log_msg.msg_type = LOG_ATT2_MSG; + log_msg.body.log_ATT.roll = buf.att.roll_sec; + log_msg.body.log_ATT.pitch = buf.att.pitch_sec; + log_msg.body.log_ATT.yaw = buf.att.yaw_sec; + log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec; + log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec; + log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec; + log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0]; + log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1]; + log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2]; + LOGBUFFER_WRITE_AND_COUNT(ATT); } /* --- ATTITUDE SETPOINT --- */ @@ -1406,6 +1428,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ATTC); } + /* --- ACTUATOR CONTROL FW VTOL --- */ + if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) { + log_msg.msg_type = LOG_ATC1_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); + } + /* --- LOCAL POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) { log_msg.msg_type = LOG_LPOS_MSG; @@ -1511,11 +1543,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { log_msg.msg_type = LOG_FLOW_MSG; - log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x; - log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y; - log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m; - log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m; - log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature; + log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral; + log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral; + log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral; + log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan; + log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral; + log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral; log_msg.body.log_FLOW.quality = buf.flow.quality; log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; LOGBUFFER_WRITE_AND_COUNT(FLOW); |