Skip to content

Zach/distance sensor hil #51

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: voxl-dev
Choose a base branch
from
Open

Zach/distance sensor hil #51

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5ca90d5
compiling
Dec 22, 2023
0619772
update
Dec 24, 2023
a1ccc5e
update to dsp hitl
ZacharyLowell Dec 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 39 additions & 1 deletion boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/esc_status.h>

#include <uORB/topics/distance_sensor.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
Expand Down Expand Up @@ -98,6 +98,7 @@ bool _send_mag = false;

uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
Expand Down Expand Up @@ -167,6 +168,7 @@ void handle_message_hil_gps_dsp(mavlink_message_t *msg);
void handle_message_odometry_dsp(mavlink_message_t *msg);
void handle_message_vision_position_estimate_dsp(mavlink_message_t *msg);
void handle_message_command_long_dsp(mavlink_message_t *msg);
void handle_message_distance_sensor_dsp(mavlink_message_t * msg);

void handle_message_dsp(mavlink_message_t *msg);
void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg);
Expand All @@ -188,6 +190,9 @@ handle_message_dsp(mavlink_message_t *msg)
case MAVLINK_MSG_ID_ODOMETRY:
handle_message_odometry_dsp(msg);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
handle_message_distance_sensor_dsp(msg);
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
handle_message_command_long_dsp(msg);
break;
Expand Down Expand Up @@ -420,6 +425,39 @@ void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
_esc_status_pub.publish(esc_status);
}

void
handle_message_distance_sensor_dsp(mavlink_message_t *msg)
{
mavlink_distance_sensor_t dist_sensor;
mavlink_msg_distance_sensor_decode(msg, &dist_sensor);

distance_sensor_s ds{};

device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.bus = 1;
device_id.devid_s.address = msg->sysid;
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;

ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
ds.min_distance = static_cast<float>(dist_sensor.min_distance) * 1e-2f; /* cm to m */
ds.max_distance = static_cast<float>(dist_sensor.max_distance) * 1e-2f; /* cm to m */
ds.current_distance = static_cast<float>(dist_sensor.current_distance) * 1e-2f; /* cm to m */
ds.variance = dist_sensor.covariance * 1e-4f; /* cm^2 to m^2 */
ds.h_fov = dist_sensor.horizontal_fov;
ds.v_fov = dist_sensor.vertical_fov;
ds.q[0] = dist_sensor.quaternion[0];
ds.q[1] = dist_sensor.quaternion[1];
ds.q[2] = dist_sensor.quaternion[2];
ds.q[3] = dist_sensor.quaternion[3];
ds.type = dist_sensor.type;
ds.device_id = device_id.devid;
ds.orientation = dist_sensor.orientation;

ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99;
_distance_sensor_pub.publish(ds);
}


void
handle_message_command_long_dsp(mavlink_message_t *msg)
Expand Down