diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 1 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 47 | ||||
-rw-r--r-- | src/modules/mavlink/module.mk | 2 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 18 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 5 |
5 files changed, 56 insertions, 17 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ed3f4001..c90b4e2de 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1942,6 +1942,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); + configure_stream("DISTANCE_SENSOR", 0.5f); break; case MAVLINK_MODE_CAMERA: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 47603b390..e608bf787 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -72,6 +72,7 @@ #include <uORB/topics/navigation_capabilities.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> +#include <drivers/drv_range_finder.h> #include "mavlink_messages.h" @@ -1271,6 +1272,51 @@ protected: } }; +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + const char *get_name() + { + return "DISTANCE_SENSOR"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamDistanceSensor(); + } + +private: + MavlinkOrbSubscription *range_sub; + struct range_finder_report *range; + +protected: + void subscribe(Mavlink *mavlink) + { + range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); + range = (struct range_finder_report *)range_sub->get_data(); + } + + void send(const hrt_abstime t) + { + (void)range_sub->update(t); + + uint8_t type; + + switch (range->type) { + case RANGE_FINDER_TYPE_LASER: + type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + uint8_t id = 0; + uint8_t orientation = 0; + uint8_t covariance = 20; + + mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, + range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1297,5 +1343,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamDistanceSensor(), nullptr }; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index c44354ff0..dcca11977 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -47,4 +47,4 @@ SRCS += mavlink_main.cpp \ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink -MAXOPTIMIZATION = -Os +MAXOPTIMIZATION = -Os diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1bf6f1de3..611491cf9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -908,9 +908,6 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; - /* track changes in distance status */ - bool dist_bottom_present = false; - /* enable logging on start if needed */ if (log_on_start) { /* check GPS topic to get GPS time */ @@ -1099,6 +1096,8 @@ int sdlog2_thread_main(int argc, char *argv[]) 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.ground_dist = buf.local_pos.dist_bottom; + log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate; 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; @@ -1108,19 +1107,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; + log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); LOGBUFFER_WRITE_AND_COUNT(LPOS); - - if (buf.local_pos.dist_bottom_valid) { - dist_bottom_present = true; - } - - if (dist_bottom_present) { - log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom; - log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate; - log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } } /* --- LOCAL POSITION SETPOINT --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d0585df39..2538dcd2f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -101,6 +101,8 @@ struct log_LPOS_s { float x; float y; float z; + float ground_dist; + float ground_dist_rate; float vx; float vy; float vz; @@ -110,6 +112,7 @@ struct log_LPOS_s { uint8_t xy_flags; uint8_t z_flags; uint8_t landed; + uint8_t ground_dist_flags; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -343,7 +346,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), 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"), |