forked from Archive/PX4-Autopilot
boards/modalai/voxl2: Add new capabilities to Qurt platform HITL driver
* Added new sensor control options and test capability in dsp_hitl * HITL working in VIO mode only * Fixed units on GPS HIL input
This commit is contained in:
Eric Katzfey
GitHub
parent
f4ebfa6130
commit
18d53c3bfd
|
|
@ -62,6 +62,8 @@
|
|||
#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 <uORB/topics/sensor_optical_flow.h>
|
||||
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
|
|
@ -88,21 +90,22 @@ static bool _is_running = false;
|
|||
volatile bool _task_should_exit = false;
|
||||
static px4_task_t _task_handle = -1;
|
||||
int _uart_fd = -1;
|
||||
bool debug = false;
|
||||
bool _debug = false;
|
||||
std::string port = "2";
|
||||
int baudrate = 921600;
|
||||
const unsigned mode_flag_custom = 1;
|
||||
const unsigned mode_flag_armed = 128;
|
||||
bool _send_gps = false;
|
||||
bool _send_mag = false;
|
||||
bool _send_distance = 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::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)};
|
||||
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
|
||||
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
|
||||
uORB::Publication<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
|
||||
uORB::Publication<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
|
||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||
|
||||
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
|
||||
|
|
@ -128,13 +131,42 @@ float x_gyro = 0;
|
|||
float y_gyro = 0;
|
||||
float z_gyro = 0;
|
||||
uint64_t gyro_accel_time = 0;
|
||||
bool _use_software_mav_throttling{false};
|
||||
|
||||
int heartbeat_counter = 0;
|
||||
int imu_counter = 0;
|
||||
int hil_sensor_counter = 0;
|
||||
int vision_msg_counter = 0;
|
||||
int gps_counter = 0;
|
||||
// Status counters
|
||||
uint32_t heartbeat_received_counter = 0;
|
||||
uint32_t heartbeat_sent_counter = 0;
|
||||
uint32_t imu_counter = 0;
|
||||
uint32_t hil_sensor_counter = 0;
|
||||
uint32_t mag_counter = 0;
|
||||
uint32_t baro_counter = 0;
|
||||
uint32_t actuator_sent_counter = 0;
|
||||
uint32_t odometry_received_counter = 0;
|
||||
uint32_t odometry_sent_counter = 0;
|
||||
uint32_t gps_received_counter = 0;
|
||||
uint32_t gps_sent_counter = 0;
|
||||
uint32_t distance_received_counter = 0;
|
||||
uint32_t distance_sent_counter = 0;
|
||||
uint32_t flow_received_counter = 0;
|
||||
uint32_t flow_sent_counter = 0;
|
||||
uint32_t unknown_msg_received_counter = 0;
|
||||
|
||||
enum class position_source {GPS, VIO, FLOW, NUM_POSITION_SOURCES};
|
||||
|
||||
struct position_source_data_s {
|
||||
char label[8];
|
||||
bool send;
|
||||
bool fail;
|
||||
uint32_t failure_duration;
|
||||
uint64_t failure_duration_start;
|
||||
} position_source_data[(int) position_source::NUM_POSITION_SOURCES] = {
|
||||
{"GPS", false, false, 0, 0},
|
||||
{"VIO", false, false, 0, 0},
|
||||
{"FLOW", false, false, 0, 0}
|
||||
};
|
||||
|
||||
uint64_t first_sensor_msg_timestamp = 0;
|
||||
uint64_t first_sensor_report_timestamp = 0;
|
||||
uint64_t last_sensor_report_timestamp = 0;
|
||||
|
||||
vehicle_status_s _vehicle_status{};
|
||||
vehicle_control_mode_s _control_mode{};
|
||||
|
|
@ -144,7 +176,6 @@ battery_status_s _battery_status{};
|
|||
sensor_accel_fifo_s accel_fifo{};
|
||||
sensor_gyro_fifo_s gyro_fifo{};
|
||||
|
||||
|
||||
int openPort(const char *dev, speed_t speed);
|
||||
int closePort();
|
||||
|
||||
|
|
@ -153,7 +184,8 @@ int writeResponse(void *buf, size_t len);
|
|||
|
||||
int start(int argc, char *argv[]);
|
||||
int stop();
|
||||
int get_status();
|
||||
void print_status();
|
||||
void clear_status_counters();
|
||||
bool isOpen() { return _uart_fd >= 0; };
|
||||
|
||||
void usage();
|
||||
|
|
@ -163,50 +195,65 @@ void *send_actuator(void *);
|
|||
void send_actuator_data();
|
||||
|
||||
void handle_message_hil_sensor_dsp(mavlink_message_t *msg);
|
||||
void handle_message_hil_optical_flow(mavlink_message_t *msg);
|
||||
void handle_message_distance_sensor(mavlink_message_t *msg);
|
||||
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_dsp(mavlink_message_t *msg);
|
||||
void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg);
|
||||
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control);
|
||||
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control);
|
||||
|
||||
void
|
||||
handle_message_dsp(mavlink_message_t *msg)
|
||||
{
|
||||
switch (msg->msgid) {
|
||||
case MAVLINK_MSG_ID_HIL_SENSOR:
|
||||
hil_sensor_counter++;
|
||||
handle_message_hil_sensor_dsp(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HIL_GPS:
|
||||
if (_send_gps) { handle_message_hil_gps_dsp(msg); }
|
||||
gps_received_counter++;
|
||||
|
||||
break;
|
||||
if (position_source_data[(int) position_source::GPS].send) { handle_message_hil_gps_dsp(msg); }
|
||||
|
||||
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
|
||||
handle_message_vision_position_estimate_dsp(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_ODOMETRY:
|
||||
handle_message_odometry_dsp(msg);
|
||||
break;
|
||||
odometry_received_counter++;
|
||||
|
||||
if (position_source_data[(int) position_source::VIO].send) { handle_message_odometry_dsp(msg); }
|
||||
|
||||
case MAVLINK_MSG_ID_COMMAND_LONG:
|
||||
handle_message_command_long_dsp(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
PX4_DEBUG("Heartbeat msg received");
|
||||
heartbeat_received_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Heartbeat msg received"); }
|
||||
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_SYSTEM_TIME:
|
||||
PX4_DEBUG("MAVLINK SYSTEM TIME");
|
||||
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
|
||||
flow_received_counter++;
|
||||
|
||||
if (position_source_data[(int) position_source::FLOW].send) { handle_message_hil_optical_flow(msg); }
|
||||
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
|
||||
distance_received_counter++;
|
||||
|
||||
if (_send_distance) { handle_message_distance_sensor(msg); }
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_DEBUG("Unknown msg ID: %d", msg->msgid);
|
||||
unknown_msg_received_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Unknown msg ID: %d", msg->msgid); }
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -228,7 +275,6 @@ void send_actuator_data()
|
|||
bool first_sent = false;
|
||||
|
||||
while (true) {
|
||||
|
||||
bool controls_updated = false;
|
||||
(void) orb_check(_vehicle_control_mode_sub_, &controls_updated);
|
||||
|
||||
|
|
@ -239,45 +285,50 @@ void send_actuator_data()
|
|||
bool actuator_updated = false;
|
||||
(void) orb_check(_actuator_outputs_sub, &actuator_updated);
|
||||
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
|
||||
if (actuator_updated) {
|
||||
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
|
||||
px4_lockstep_wait_for_components();
|
||||
|
||||
if (_actuator_outputs.timestamp > 0) {
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
previous_timestamp = _actuator_outputs.timestamp;
|
||||
previous_uorb_timestamp = _actuator_outputs.timestamp;
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
|
||||
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
||||
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
|
||||
|
||||
actuator_sent_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||
|
||||
first_sent = true;
|
||||
send_esc_telemetry_dsp(hil_act_control);
|
||||
|
||||
send_esc_status(hil_act_control);
|
||||
}
|
||||
|
||||
} else if (!actuator_updated && first_sent && differential > 4000) {
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
} else if (! actuator_updated && first_sent && differential > 4000) {
|
||||
previous_timestamp = hrt_absolute_time();
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
||||
//PX4_INFO("Sending from NOT UPDTE AND TIMEOUT: %i", differential);
|
||||
|
||||
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
|
||||
send_esc_telemetry_dsp(hil_act_control);
|
||||
actuator_sent_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||
|
||||
send_esc_status(hil_act_control);
|
||||
}
|
||||
|
||||
differential = hrt_absolute_time() - previous_timestamp;
|
||||
|
||||
px4_usleep(1000);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -287,14 +338,10 @@ void task_main(int argc, char *argv[])
|
|||
int myoptind = 1;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "vsdcmgp:b:", &myoptind, &myoptarg)) != EOF) {
|
||||
while ((ch = px4_getopt(argc, argv, "odmghfp:b:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 's':
|
||||
_use_software_mav_throttling = true;
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
debug = true;
|
||||
_debug = true;
|
||||
break;
|
||||
|
||||
case 'p':
|
||||
|
|
@ -310,7 +357,19 @@ void task_main(int argc, char *argv[])
|
|||
break;
|
||||
|
||||
case 'g':
|
||||
_send_gps = true;
|
||||
position_source_data[(int) position_source::GPS].send = true;
|
||||
break;
|
||||
|
||||
case 'o':
|
||||
position_source_data[(int) position_source::VIO].send = true;
|
||||
break;
|
||||
|
||||
case 'h':
|
||||
_send_distance = true;
|
||||
break;
|
||||
|
||||
case 'f':
|
||||
position_source_data[(int) position_source::FLOW].send = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -319,11 +378,13 @@ void task_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
const char *charport = port.c_str();
|
||||
int openRetval = openPort(charport, (speed_t) baudrate);
|
||||
int open = isOpen();
|
||||
(void) openPort(charport, (speed_t) baudrate);
|
||||
|
||||
if (open) {
|
||||
PX4_ERR("Port is open: %d", openRetval);
|
||||
if ((_debug) && (isOpen())) { PX4_INFO("DSP HITL serial port initialized. Baudrate: %d", baudrate); }
|
||||
|
||||
if (! isOpen()) {
|
||||
PX4_ERR("DSP HITL failed to open serial port");
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t last_heartbeat_timestamp = hrt_absolute_time();
|
||||
|
|
@ -342,14 +403,11 @@ void task_main(int argc, char *argv[])
|
|||
pthread_attr_destroy(&sender_thread_attr);
|
||||
|
||||
int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
PX4_INFO("Got %d from orb_subscribe", _vehicle_status_sub);
|
||||
|
||||
_is_running = true;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
uint8_t rx_buf[1024];
|
||||
//rx_buf[511] = '\0';
|
||||
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
||||
|
|
@ -357,8 +415,8 @@ void task_main(int argc, char *argv[])
|
|||
if (got_first_sensor_msg) {
|
||||
uint64_t delta_time = timestamp - last_imu_update_timestamp;
|
||||
|
||||
if (delta_time > 15000) {
|
||||
PX4_ERR("Sending updates at %llu, delta %llu", timestamp, delta_time);
|
||||
if ((imu_counter) && (delta_time > 15000)) {
|
||||
PX4_WARN("Sending updates at %llu, delta %llu", timestamp, delta_time);
|
||||
}
|
||||
|
||||
uint64_t _px4_gyro_accel_timestamp = hrt_absolute_time();
|
||||
|
|
@ -396,7 +454,7 @@ void task_main(int argc, char *argv[])
|
|||
hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
|
||||
(void) writeResponse(&hb_newBuf, hb_newBufLen);
|
||||
last_heartbeat_timestamp = timestamp;
|
||||
heartbeat_counter++;
|
||||
heartbeat_sent_counter++;
|
||||
}
|
||||
|
||||
bool vehicle_updated = false;
|
||||
|
|
@ -416,7 +474,7 @@ void task_main(int argc, char *argv[])
|
|||
_is_running = false;
|
||||
}
|
||||
|
||||
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
|
||||
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control)
|
||||
{
|
||||
esc_status_s esc_status{};
|
||||
esc_status.timestamp = hrt_absolute_time();
|
||||
|
|
@ -448,17 +506,13 @@ void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
|
|||
_esc_status_pub.publish(esc_status);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
handle_message_command_long_dsp(mavlink_message_t *msg)
|
||||
{
|
||||
/* command */
|
||||
mavlink_command_long_t cmd_mavlink;
|
||||
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
||||
|
||||
if (debug) {
|
||||
PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command);
|
||||
}
|
||||
if (_debug) { PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command); }
|
||||
|
||||
mavlink_command_ack_t ack = {};
|
||||
ack.result = MAV_RESULT_UNSUPPORTED;
|
||||
|
|
@ -470,46 +524,140 @@ handle_message_command_long_dsp(mavlink_message_t *msg)
|
|||
uint16_t acknewBufLen = 0;
|
||||
acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message);
|
||||
int writeRetval = writeResponse(&acknewBuf, acknewBufLen);
|
||||
PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time());
|
||||
|
||||
if (_debug) { PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||
}
|
||||
|
||||
int flow_debug_counter = 0;
|
||||
|
||||
void
|
||||
handle_message_vision_position_estimate_dsp(mavlink_message_t *msg)
|
||||
handle_message_hil_optical_flow(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_vision_position_estimate_t vpe;
|
||||
mavlink_msg_vision_position_estimate_decode(msg, &vpe);
|
||||
mavlink_hil_optical_flow_t flow;
|
||||
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||
|
||||
// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
|
||||
vehicle_odometry_s odom{};
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
odom.timestamp_sample = timestamp;
|
||||
if ((_debug) && (!(flow_debug_counter % 10))) {
|
||||
PX4_INFO("optflow: time: %llu, quality %d", flow.time_usec, (int) flow.quality);
|
||||
PX4_INFO("optflow: x: %.2f y: %.2f", (double) flow.integrated_x, (double) flow.integrated_y);
|
||||
}
|
||||
|
||||
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
|
||||
odom.position[0] = vpe.x;
|
||||
odom.position[1] = vpe.y;
|
||||
odom.position[2] = vpe.z;
|
||||
flow_debug_counter++;
|
||||
|
||||
const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
|
||||
q.copyTo(odom.q);
|
||||
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_FLOW_DEVTYPE_SIM;
|
||||
|
||||
// VISION_POSITION_ESTIMATE covariance
|
||||
// Row-major representation of pose 6x6 cross-covariance matrix upper right triangle
|
||||
// (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
|
||||
// If unknown, assign NaN value to first element in the array.
|
||||
odom.position_variance[0] = vpe.covariance[0]; // X row 0, col 0
|
||||
odom.position_variance[1] = vpe.covariance[6]; // Y row 1, col 1
|
||||
odom.position_variance[2] = vpe.covariance[11]; // Z row 2, col 2
|
||||
sensor_optical_flow_s sensor_optical_flow{};
|
||||
|
||||
odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3
|
||||
odom.orientation_variance[1] = vpe.covariance[18]; // P row 4, col 4
|
||||
odom.orientation_variance[2] = vpe.covariance[20]; // Y row 5, col 5
|
||||
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
|
||||
sensor_optical_flow.device_id = device_id.devid;
|
||||
|
||||
odom.reset_counter = vpe.reset_counter;
|
||||
sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
|
||||
sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
|
||||
|
||||
odom.timestamp = hrt_absolute_time();
|
||||
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
|
||||
sensor_optical_flow.quality = flow.quality;
|
||||
|
||||
_visual_odometry_pub.publish(odom);
|
||||
vision_msg_counter++;
|
||||
int index = (int) position_source::FLOW;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("Optical flow failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
sensor_optical_flow.quality = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
sensor_optical_flow.quality = 0;
|
||||
}
|
||||
}
|
||||
|
||||
const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro);
|
||||
|
||||
if (integrated_gyro.isAllFinite()) {
|
||||
integrated_gyro.copyTo(sensor_optical_flow.delta_angle);
|
||||
sensor_optical_flow.delta_angle_available = true;
|
||||
}
|
||||
|
||||
sensor_optical_flow.max_flow_rate = NAN;
|
||||
sensor_optical_flow.min_ground_distance = NAN;
|
||||
sensor_optical_flow.max_ground_distance = NAN;
|
||||
|
||||
// Use distance value for distance sensor topic
|
||||
// if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) {
|
||||
// // Positive value (including zero): distance known. Negative value: Unknown distance.
|
||||
// sensor_optical_flow.distance_m = flow.distance;
|
||||
// sensor_optical_flow.distance_available = true;
|
||||
// }
|
||||
|
||||
// Emulate voxl-flow-server where distance comes in a separate
|
||||
// distance sensor topic message
|
||||
sensor_optical_flow.distance_m = 0.0f;
|
||||
sensor_optical_flow.distance_available = false;
|
||||
|
||||
sensor_optical_flow.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_optical_flow_pub.publish(sensor_optical_flow);
|
||||
|
||||
flow_sent_counter++;
|
||||
}
|
||||
|
||||
int distance_debug_counter = 0;
|
||||
|
||||
void handle_message_distance_sensor(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_distance_sensor_t dist_sensor;
|
||||
mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
|
||||
|
||||
if ((_debug) && (!(distance_debug_counter % 10))) {
|
||||
PX4_INFO("distance: time: %u, quality: %u, height: %u",
|
||||
dist_sensor.time_boot_ms, dist_sensor.signal_quality,
|
||||
dist_sensor.current_distance);
|
||||
}
|
||||
|
||||
distance_debug_counter++;
|
||||
|
||||
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;
|
||||
|
||||
// MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown
|
||||
// quality value. Also it comes normalised between 1 and 100 while the uORB
|
||||
// signal quality is normalised between 0 and 100.
|
||||
ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99;
|
||||
|
||||
_distance_sensor_pub.publish(ds);
|
||||
|
||||
distance_sent_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -518,6 +666,8 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
|
|||
mavlink_odometry_t odom_in;
|
||||
mavlink_msg_odometry_decode(msg, &odom_in);
|
||||
|
||||
odometry_sent_counter++;
|
||||
|
||||
// fill vehicle_odometry from Mavlink ODOMETRY
|
||||
vehicle_odometry_s odom{};
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
|
@ -699,6 +849,28 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
|
|||
odom.reset_counter = odom_in.reset_counter;
|
||||
odom.quality = odom_in.quality;
|
||||
|
||||
int index = (int) position_source::VIO;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("VIO failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
odom.quality = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
odom.quality = 0;
|
||||
}
|
||||
}
|
||||
|
||||
switch (odom_in.estimator_type) {
|
||||
case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
|
||||
case MAV_ESTIMATOR_TYPE_NAIVE:
|
||||
|
|
@ -745,10 +917,6 @@ void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg)
|
|||
msg->mode = mode_flag_custom;
|
||||
msg->mode |= (armed) ? mode_flag_armed : 0;
|
||||
msg->flags = 0;
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
msg->flags |= 1;
|
||||
#endif
|
||||
}
|
||||
|
||||
int openPort(const char *dev, speed_t speed)
|
||||
|
|
@ -759,7 +927,8 @@ int openPort(const char *dev, speed_t speed)
|
|||
}
|
||||
|
||||
_uart_fd = qurt_uart_open(dev, speed);
|
||||
PX4_DEBUG("qurt_uart_opened");
|
||||
|
||||
if (_debug) { PX4_INFO("qurt_uart_opened"); }
|
||||
|
||||
if (_uart_fd < 0) {
|
||||
PX4_ERR("Error opening port: %s (%i)", dev, errno);
|
||||
|
|
@ -840,25 +1009,50 @@ int stop()
|
|||
|
||||
void usage()
|
||||
{
|
||||
PX4_INFO("Usage: dsp_hitl {start|info|status|stop}");
|
||||
PX4_INFO("Usage: dsp_hitl {start|status|clear|failure|stop}");
|
||||
PX4_INFO(" failure <source> <duration>");
|
||||
PX4_INFO(" source: gps, vio, flow");
|
||||
PX4_INFO(" duration: 0 (toggle state), else seconds");
|
||||
}
|
||||
|
||||
int get_status()
|
||||
void print_status()
|
||||
{
|
||||
PX4_INFO("Running: %s", _is_running ? "yes" : "no");
|
||||
PX4_INFO("Status of IMU_Data counter: %i", imu_counter);
|
||||
PX4_INFO("Value of current accel x, y, z data: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
|
||||
PX4_INFO("Value of current gyro x, y, z data: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
||||
PX4_INFO("Value of HIL_Sensor counter: %i", hil_sensor_counter);
|
||||
PX4_INFO("Value of Heartbeat counter: %i", heartbeat_counter);
|
||||
PX4_INFO("Value of Vision data counter: %i", vision_msg_counter);
|
||||
PX4_INFO("Value of GPS Data counter: %i", gps_counter);
|
||||
return 0;
|
||||
PX4_INFO("HIL Sensor received: %i", hil_sensor_counter);
|
||||
PX4_INFO("IMU updates: %i", imu_counter);
|
||||
PX4_INFO("\tCurrent accel x, y, z: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
|
||||
PX4_INFO("\tCurrent gyro x, y, z: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
||||
PX4_INFO("Magnetometer sent: %i", mag_counter);
|
||||
PX4_INFO("Barometer sent: %i", baro_counter);
|
||||
PX4_INFO("Heartbeat received: %i, sent: %i", heartbeat_received_counter, heartbeat_sent_counter);
|
||||
PX4_INFO("Odometry received: %i, sent: %i", odometry_received_counter, odometry_sent_counter);
|
||||
PX4_INFO("GPS received: %i, sent: %i", gps_received_counter, gps_sent_counter);
|
||||
PX4_INFO("Distance sensor received: %i, sent: %i", distance_received_counter, distance_sent_counter);
|
||||
PX4_INFO("Optical flow received: %i, sent: %i", flow_received_counter, flow_sent_counter);
|
||||
PX4_INFO("Actuator updates sent: %i", actuator_sent_counter);
|
||||
PX4_INFO("Unknown messages received: %i", unknown_msg_received_counter);
|
||||
}
|
||||
|
||||
uint64_t first_sensor_msg_timestamp = 0;
|
||||
uint64_t first_sensor_report_timestamp = 0;
|
||||
uint64_t last_sensor_report_timestamp = 0;
|
||||
void
|
||||
clear_status_counters()
|
||||
{
|
||||
heartbeat_received_counter = 0;
|
||||
heartbeat_sent_counter = 0;
|
||||
imu_counter = 0;
|
||||
hil_sensor_counter = 0;
|
||||
mag_counter = 0;
|
||||
baro_counter = 0;
|
||||
actuator_sent_counter = 0;
|
||||
odometry_received_counter = 0;
|
||||
odometry_sent_counter = 0;
|
||||
gps_received_counter = 0;
|
||||
gps_sent_counter = 0;
|
||||
distance_received_counter = 0;
|
||||
distance_sent_counter = 0;
|
||||
flow_received_counter = 0;
|
||||
flow_sent_counter = 0;
|
||||
unknown_msg_received_counter = 0;
|
||||
}
|
||||
|
||||
void
|
||||
handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
||||
|
|
@ -928,6 +1122,8 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
|||
}
|
||||
|
||||
_px4_mag->update(gyro_accel_time, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag);
|
||||
|
||||
mag_counter++;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -942,17 +1138,8 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
|||
sensor_baro.error_count = 0;
|
||||
sensor_baro.timestamp = hrt_absolute_time();
|
||||
_sensor_baro_pub.publish(sensor_baro);
|
||||
}
|
||||
|
||||
// differential pressure
|
||||
if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
|
||||
differential_pressure_s report{};
|
||||
report.timestamp_sample = gyro_accel_time;
|
||||
report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
|
||||
report.temperature = hil_sensor.temperature;
|
||||
report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // hPa to Pa
|
||||
report.timestamp = hrt_absolute_time();
|
||||
_differential_pressure_pub.publish(report);
|
||||
baro_counter++;
|
||||
}
|
||||
|
||||
// battery status
|
||||
|
|
@ -970,7 +1157,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
|||
|
||||
_battery_pub.publish(hil_battery_status);
|
||||
}
|
||||
hil_sensor_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -989,15 +1175,41 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
|
||||
gps.device_id = device_id.devid;
|
||||
|
||||
gps.latitude_deg = hil_gps.lat;
|
||||
gps.longitude_deg = hil_gps.lon;
|
||||
gps.altitude_msl_m = hil_gps.alt;
|
||||
gps.altitude_ellipsoid_m = hil_gps.alt;
|
||||
gps.latitude_deg = hil_gps.lat * 1e-7;
|
||||
gps.longitude_deg = hil_gps.lon * 1e-7;
|
||||
gps.altitude_msl_m = hil_gps.alt * 1e-3;
|
||||
gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3;
|
||||
|
||||
gps.s_variance_m_s = 0.25f;
|
||||
gps.c_variance_rad = 0.5f;
|
||||
|
||||
gps.satellites_used = hil_gps.satellites_visible;
|
||||
gps.fix_type = hil_gps.fix_type;
|
||||
|
||||
int index = (int) position_source::GPS;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("GPS failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
gps.satellites_used = 1;
|
||||
gps.fix_type = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
gps.satellites_used = 1;
|
||||
gps.fix_type = 0;
|
||||
}
|
||||
}
|
||||
|
||||
gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
|
||||
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
|
||||
|
||||
|
|
@ -1021,7 +1233,6 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
gps.timestamp_time_relative = 0;
|
||||
gps.time_utc_usec = hil_gps.time_usec;
|
||||
|
||||
gps.satellites_used = hil_gps.satellites_visible;
|
||||
|
||||
gps.heading = NAN;
|
||||
gps.heading_offset = NAN;
|
||||
|
|
@ -1029,10 +1240,51 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
gps.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_gps_pub.publish(gps);
|
||||
gps_counter++;
|
||||
|
||||
gps_sent_counter++;
|
||||
}
|
||||
|
||||
int
|
||||
process_failure(dsp_hitl::position_source src, int duration)
|
||||
{
|
||||
if (src >= position_source::NUM_POSITION_SOURCES) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
int index = (int) src;
|
||||
|
||||
if (position_source_data[index].send) {
|
||||
if (duration <= 0) {
|
||||
// Toggle state
|
||||
if (position_source_data[index].fail) {
|
||||
PX4_INFO("Ending indefinite %s failure", position_source_data[index].label);
|
||||
position_source_data[index].fail = false;
|
||||
|
||||
} else {
|
||||
PX4_INFO("Starting indefinite %s failure", position_source_data[index].label);
|
||||
position_source_data[index].fail = true;
|
||||
}
|
||||
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
PX4_INFO("%s failure for %d seconds", position_source_data[index].label, duration);
|
||||
position_source_data[index].fail = true;
|
||||
position_source_data[index].failure_duration = duration;
|
||||
position_source_data[index].failure_duration_start = hrt_absolute_time();
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("%s not active, cannot create failure", position_source_data[index].label);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // End dsp_hitl namespace
|
||||
|
||||
int dsp_hitl_main(int argc, char *argv[])
|
||||
{
|
||||
int myoptind = 1;
|
||||
|
|
@ -1044,20 +1296,47 @@ int dsp_hitl_main(int argc, char *argv[])
|
|||
|
||||
const char *verb = argv[myoptind];
|
||||
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return dsp_hitl::start(argc - 1, argv + 1);
|
||||
}
|
||||
|
||||
else if (!strcmp(verb, "stop")) {
|
||||
} else if (!strcmp(verb, "stop")) {
|
||||
return dsp_hitl::stop();
|
||||
|
||||
} else if (!strcmp(verb, "status")) {
|
||||
dsp_hitl::print_status();
|
||||
return 0;
|
||||
|
||||
} else if (!strcmp(verb, "clear")) {
|
||||
dsp_hitl::clear_status_counters();
|
||||
return 0;
|
||||
|
||||
} else if (!strcmp(verb, "failure")) {
|
||||
if (argc != 4) {
|
||||
dsp_hitl::usage();
|
||||
return 1;
|
||||
}
|
||||
|
||||
else if (!strcmp(verb, "status")) {
|
||||
return dsp_hitl::get_status();
|
||||
const char *source = argv[myoptind + 1];
|
||||
int duration = atoi(argv[myoptind + 2]);
|
||||
|
||||
if (!strcmp(source, "gps")) {
|
||||
return dsp_hitl::process_failure(dsp_hitl::position_source::GPS, duration);
|
||||
|
||||
} else if (!strcmp(source, "vio")) {
|
||||
return dsp_hitl::process_failure(dsp_hitl::position_source::VIO, duration);
|
||||
|
||||
} else if (!strcmp(source, "flow")) {
|
||||
return dsp_hitl::process_failure(dsp_hitl::position_source::FLOW, duration);
|
||||
|
||||
} else {
|
||||
PX4_ERR("Unknown failure source %s, duration %d", source, duration);
|
||||
dsp_hitl::usage();
|
||||
return 1;
|
||||
}
|
||||
|
||||
else {
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
dsp_hitl::usage();
|
||||
return 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -82,6 +82,8 @@ load_mon start
|
|||
microdds_client start -t udp -h 127.0.0.1 -p 8888
|
||||
|
||||
qshell pwm_out_sim start -m hil
|
||||
# g = gps, m = mag, o = odometry (vio), h = distance sensor, f = optic flow
|
||||
# qshell dsp_hitl start -g -m -o -h -f
|
||||
qshell dsp_hitl start -g -m
|
||||
|
||||
# start the onboard fast link to connect to voxl-mavlink-server
|
||||
|
|
|
|||
Loading…
Reference in New Issue