diff options
| author | Nuno Marques <n.marques21@hotmail.com> | 2014-08-25 11:20:55 +0100 |
|---|---|---|
| committer | Nuno Marques <n.marques21@hotmail.com> | 2014-08-25 11:20:55 +0100 |
| commit | ec8438bdcab31c566f91869140505b506a4fcaf8 (patch) | |
| tree | 2e5d70c6cef702d2e3ba5650706c7abea4ccae9c | |
| parent | 60799e51558e6259a7a2d20d765a4f8d29b88cc5 (diff) | |
| download | px4-firmware-ec8438bdcab31c566f91869140505b506a4fcaf8.tar.gz px4-firmware-ec8438bdcab31c566f91869140505b506a4fcaf8.tar.bz2 px4-firmware-ec8438bdcab31c566f91869140505b506a4fcaf8.zip | |
sdlog2: added vision estimate logging
| -rw-r--r-- | src/modules/sdlog2/sdlog2.c | 25 |
1 files changed, 17 insertions, 8 deletions
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6c4b49452..59765415a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -934,6 +934,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; + struct vehicle_vision_position_s vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -984,6 +985,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; + struct log_VISN_s log_VISN; struct log_GS0A_s log_GS0A; struct log_GS0B_s log_GS0B; struct log_GS1A_s log_GS1A; @@ -1013,6 +1015,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int gps_pos_sub; int sat_info_sub; int vicon_pos_sub; + int vision_pos_sub; int flow_sub; int rc_sub; int airspeed_sub; @@ -1043,6 +1046,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + subs.vision_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); @@ -1459,6 +1463,18 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; LOGBUFFER_WRITE_AND_COUNT(VICN); } + + /* --- VISION POSITION --- */ + if (copy_if_updated(ORB_ID(vehicle_vision_position), subs.vision_pos_sub, &buf.vision_pos)) { + log_msg.msg_type = LOG_VISN_MSG; + log_msg.body.log_VISN.x = buf.vision_pos.x; + log_msg.body.log_VISN.y = buf.vision_pos.y; + log_msg.body.log_VISN.z = buf.vision_pos.z; + log_msg.body.log_VISN.pitch = buf.vision_pos.pitch; + log_msg.body.log_VISN.roll = buf.vision_pos.roll; + log_msg.body.log_VISN.yaw = buf.vision_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VISN); + } /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { @@ -1565,14 +1581,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- BOTTOM DISTANCE --- */ - if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { - log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.range_finder.distance; - log_msg.body.log_DIST.bottom_rate = 0.0f; - log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } + /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { |