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:
parent
f4ebfa6130
commit
18d53c3bfd
|
@ -62,6 +62,8 @@
|
||||||
#include <uORB/topics/vehicle_odometry.h>
|
#include <uORB/topics/vehicle_odometry.h>
|
||||||
#include <uORB/topics/sensor_baro.h>
|
#include <uORB/topics/sensor_baro.h>
|
||||||
#include <uORB/topics/esc_status.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/accelerometer/PX4Accelerometer.hpp>
|
||||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||||
|
@ -88,21 +90,22 @@ static bool _is_running = false;
|
||||||
volatile bool _task_should_exit = false;
|
volatile bool _task_should_exit = false;
|
||||||
static px4_task_t _task_handle = -1;
|
static px4_task_t _task_handle = -1;
|
||||||
int _uart_fd = -1;
|
int _uart_fd = -1;
|
||||||
bool debug = false;
|
bool _debug = false;
|
||||||
std::string port = "2";
|
std::string port = "2";
|
||||||
int baudrate = 921600;
|
int baudrate = 921600;
|
||||||
const unsigned mode_flag_custom = 1;
|
const unsigned mode_flag_custom = 1;
|
||||||
const unsigned mode_flag_armed = 128;
|
const unsigned mode_flag_armed = 128;
|
||||||
bool _send_gps = false;
|
|
||||||
bool _send_mag = false;
|
bool _send_mag = false;
|
||||||
|
bool _send_distance = false;
|
||||||
|
|
||||||
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
|
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<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> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
|
||||||
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_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::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<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)};
|
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||||
|
|
||||||
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
|
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
|
||||||
|
@ -128,13 +131,42 @@ float x_gyro = 0;
|
||||||
float y_gyro = 0;
|
float y_gyro = 0;
|
||||||
float z_gyro = 0;
|
float z_gyro = 0;
|
||||||
uint64_t gyro_accel_time = 0;
|
uint64_t gyro_accel_time = 0;
|
||||||
bool _use_software_mav_throttling{false};
|
|
||||||
|
|
||||||
int heartbeat_counter = 0;
|
// Status counters
|
||||||
int imu_counter = 0;
|
uint32_t heartbeat_received_counter = 0;
|
||||||
int hil_sensor_counter = 0;
|
uint32_t heartbeat_sent_counter = 0;
|
||||||
int vision_msg_counter = 0;
|
uint32_t imu_counter = 0;
|
||||||
int gps_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_status_s _vehicle_status{};
|
||||||
vehicle_control_mode_s _control_mode{};
|
vehicle_control_mode_s _control_mode{};
|
||||||
|
@ -144,7 +176,6 @@ battery_status_s _battery_status{};
|
||||||
sensor_accel_fifo_s accel_fifo{};
|
sensor_accel_fifo_s accel_fifo{};
|
||||||
sensor_gyro_fifo_s gyro_fifo{};
|
sensor_gyro_fifo_s gyro_fifo{};
|
||||||
|
|
||||||
|
|
||||||
int openPort(const char *dev, speed_t speed);
|
int openPort(const char *dev, speed_t speed);
|
||||||
int closePort();
|
int closePort();
|
||||||
|
|
||||||
|
@ -153,7 +184,8 @@ int writeResponse(void *buf, size_t len);
|
||||||
|
|
||||||
int start(int argc, char *argv[]);
|
int start(int argc, char *argv[]);
|
||||||
int stop();
|
int stop();
|
||||||
int get_status();
|
void print_status();
|
||||||
|
void clear_status_counters();
|
||||||
bool isOpen() { return _uart_fd >= 0; };
|
bool isOpen() { return _uart_fd >= 0; };
|
||||||
|
|
||||||
void usage();
|
void usage();
|
||||||
|
@ -163,50 +195,65 @@ void *send_actuator(void *);
|
||||||
void send_actuator_data();
|
void send_actuator_data();
|
||||||
|
|
||||||
void handle_message_hil_sensor_dsp(mavlink_message_t *msg);
|
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_hil_gps_dsp(mavlink_message_t *msg);
|
||||||
void handle_message_odometry_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_command_long_dsp(mavlink_message_t *msg);
|
||||||
|
|
||||||
void handle_message_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 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
|
void
|
||||||
handle_message_dsp(mavlink_message_t *msg)
|
handle_message_dsp(mavlink_message_t *msg)
|
||||||
{
|
{
|
||||||
switch (msg->msgid) {
|
switch (msg->msgid) {
|
||||||
case MAVLINK_MSG_ID_HIL_SENSOR:
|
case MAVLINK_MSG_ID_HIL_SENSOR:
|
||||||
|
hil_sensor_counter++;
|
||||||
handle_message_hil_sensor_dsp(msg);
|
handle_message_hil_sensor_dsp(msg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_HIL_GPS:
|
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;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_ODOMETRY:
|
case MAVLINK_MSG_ID_ODOMETRY:
|
||||||
handle_message_odometry_dsp(msg);
|
odometry_received_counter++;
|
||||||
break;
|
|
||||||
|
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;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||||
PX4_DEBUG("Heartbeat msg received");
|
heartbeat_received_counter++;
|
||||||
|
|
||||||
|
if (_debug) { PX4_INFO("Heartbeat msg received"); }
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_SYSTEM_TIME:
|
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
|
||||||
PX4_DEBUG("MAVLINK SYSTEM TIME");
|
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;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
PX4_DEBUG("Unknown msg ID: %d", msg->msgid);
|
unknown_msg_received_counter++;
|
||||||
|
|
||||||
|
if (_debug) { PX4_INFO("Unknown msg ID: %d", msg->msgid); }
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -228,7 +275,6 @@ void send_actuator_data()
|
||||||
bool first_sent = false;
|
bool first_sent = false;
|
||||||
|
|
||||||
while (true) {
|
while (true) {
|
||||||
|
|
||||||
bool controls_updated = false;
|
bool controls_updated = false;
|
||||||
(void) orb_check(_vehicle_control_mode_sub_, &controls_updated);
|
(void) orb_check(_vehicle_control_mode_sub_, &controls_updated);
|
||||||
|
|
||||||
|
@ -239,45 +285,50 @@ void send_actuator_data()
|
||||||
bool actuator_updated = false;
|
bool actuator_updated = false;
|
||||||
(void) orb_check(_actuator_outputs_sub, &actuator_updated);
|
(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) {
|
if (actuator_updated) {
|
||||||
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
|
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
|
||||||
px4_lockstep_wait_for_components();
|
|
||||||
|
|
||||||
if (_actuator_outputs.timestamp > 0) {
|
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_timestamp = _actuator_outputs.timestamp;
|
||||||
previous_uorb_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);
|
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
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;
|
first_sent = true;
|
||||||
send_esc_telemetry_dsp(hil_act_control);
|
|
||||||
|
send_esc_status(hil_act_control);
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (!actuator_updated && first_sent && differential > 4000) {
|
} else if (! actuator_updated && first_sent && differential > 4000) {
|
||||||
mavlink_hil_actuator_controls_t hil_act_control;
|
|
||||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
|
||||||
previous_timestamp = hrt_absolute_time();
|
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);
|
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
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());
|
actuator_sent_counter++;
|
||||||
send_esc_telemetry_dsp(hil_act_control);
|
|
||||||
|
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;
|
differential = hrt_absolute_time() - previous_timestamp;
|
||||||
|
|
||||||
|
px4_usleep(1000);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -287,14 +338,10 @@ void task_main(int argc, char *argv[])
|
||||||
int myoptind = 1;
|
int myoptind = 1;
|
||||||
const char *myoptarg = nullptr;
|
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) {
|
switch (ch) {
|
||||||
case 's':
|
|
||||||
_use_software_mav_throttling = true;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 'd':
|
case 'd':
|
||||||
debug = true;
|
_debug = true;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 'p':
|
case 'p':
|
||||||
|
@ -310,7 +357,19 @@ void task_main(int argc, char *argv[])
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 'g':
|
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;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
|
@ -319,11 +378,13 @@ void task_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
const char *charport = port.c_str();
|
const char *charport = port.c_str();
|
||||||
int openRetval = openPort(charport, (speed_t) baudrate);
|
(void) openPort(charport, (speed_t) baudrate);
|
||||||
int open = isOpen();
|
|
||||||
|
|
||||||
if (open) {
|
if ((_debug) && (isOpen())) { PX4_INFO("DSP HITL serial port initialized. Baudrate: %d", baudrate); }
|
||||||
PX4_ERR("Port is open: %d", openRetval);
|
|
||||||
|
if (! isOpen()) {
|
||||||
|
PX4_ERR("DSP HITL failed to open serial port");
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t last_heartbeat_timestamp = hrt_absolute_time();
|
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);
|
pthread_attr_destroy(&sender_thread_attr);
|
||||||
|
|
||||||
int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||||
PX4_INFO("Got %d from orb_subscribe", _vehicle_status_sub);
|
|
||||||
|
|
||||||
_is_running = true;
|
_is_running = true;
|
||||||
|
|
||||||
while (!_task_should_exit) {
|
while (!_task_should_exit) {
|
||||||
|
|
||||||
uint8_t rx_buf[1024];
|
uint8_t rx_buf[1024];
|
||||||
//rx_buf[511] = '\0';
|
|
||||||
|
|
||||||
uint64_t timestamp = hrt_absolute_time();
|
uint64_t timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
|
@ -357,8 +415,8 @@ void task_main(int argc, char *argv[])
|
||||||
if (got_first_sensor_msg) {
|
if (got_first_sensor_msg) {
|
||||||
uint64_t delta_time = timestamp - last_imu_update_timestamp;
|
uint64_t delta_time = timestamp - last_imu_update_timestamp;
|
||||||
|
|
||||||
if (delta_time > 15000) {
|
if ((imu_counter) && (delta_time > 15000)) {
|
||||||
PX4_ERR("Sending updates at %llu, delta %llu", timestamp, delta_time);
|
PX4_WARN("Sending updates at %llu, delta %llu", timestamp, delta_time);
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t _px4_gyro_accel_timestamp = hrt_absolute_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);
|
hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
|
||||||
(void) writeResponse(&hb_newBuf, hb_newBufLen);
|
(void) writeResponse(&hb_newBuf, hb_newBufLen);
|
||||||
last_heartbeat_timestamp = timestamp;
|
last_heartbeat_timestamp = timestamp;
|
||||||
heartbeat_counter++;
|
heartbeat_sent_counter++;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool vehicle_updated = false;
|
bool vehicle_updated = false;
|
||||||
|
@ -416,7 +474,7 @@ void task_main(int argc, char *argv[])
|
||||||
_is_running = false;
|
_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_s esc_status{};
|
||||||
esc_status.timestamp = hrt_absolute_time();
|
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);
|
_esc_status_pub.publish(esc_status);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void
|
void
|
||||||
handle_message_command_long_dsp(mavlink_message_t *msg)
|
handle_message_command_long_dsp(mavlink_message_t *msg)
|
||||||
{
|
{
|
||||||
/* command */
|
|
||||||
mavlink_command_long_t cmd_mavlink;
|
mavlink_command_long_t cmd_mavlink;
|
||||||
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
||||||
|
|
||||||
if (debug) {
|
if (_debug) { PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command); }
|
||||||
PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command);
|
|
||||||
}
|
|
||||||
|
|
||||||
mavlink_command_ack_t ack = {};
|
mavlink_command_ack_t ack = {};
|
||||||
ack.result = MAV_RESULT_UNSUPPORTED;
|
ack.result = MAV_RESULT_UNSUPPORTED;
|
||||||
|
@ -470,46 +524,140 @@ handle_message_command_long_dsp(mavlink_message_t *msg)
|
||||||
uint16_t acknewBufLen = 0;
|
uint16_t acknewBufLen = 0;
|
||||||
acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message);
|
acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message);
|
||||||
int writeRetval = writeResponse(&acknewBuf, acknewBufLen);
|
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
|
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_hil_optical_flow_t flow;
|
||||||
mavlink_msg_vision_position_estimate_decode(msg, &vpe);
|
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||||
|
|
||||||
// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
|
if ((_debug) && (!(flow_debug_counter % 10))) {
|
||||||
vehicle_odometry_s odom{};
|
PX4_INFO("optflow: time: %llu, quality %d", flow.time_usec, (int) flow.quality);
|
||||||
uint64_t timestamp = hrt_absolute_time();
|
PX4_INFO("optflow: x: %.2f y: %.2f", (double) flow.integrated_x, (double) flow.integrated_y);
|
||||||
odom.timestamp_sample = timestamp;
|
}
|
||||||
|
|
||||||
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
|
flow_debug_counter++;
|
||||||
odom.position[0] = vpe.x;
|
|
||||||
odom.position[1] = vpe.y;
|
|
||||||
odom.position[2] = vpe.z;
|
|
||||||
|
|
||||||
const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
|
device::Device::DeviceId device_id;
|
||||||
q.copyTo(odom.q);
|
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
|
sensor_optical_flow_s sensor_optical_flow{};
|
||||||
// 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
|
|
||||||
|
|
||||||
odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3
|
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
|
||||||
odom.orientation_variance[1] = vpe.covariance[18]; // P row 4, col 4
|
sensor_optical_flow.device_id = device_id.devid;
|
||||||
odom.orientation_variance[2] = vpe.covariance[20]; // Y row 5, col 5
|
|
||||||
|
|
||||||
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);
|
int index = (int) position_source::FLOW;
|
||||||
vision_msg_counter++;
|
|
||||||
|
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
|
void
|
||||||
|
@ -518,6 +666,8 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
|
||||||
mavlink_odometry_t odom_in;
|
mavlink_odometry_t odom_in;
|
||||||
mavlink_msg_odometry_decode(msg, &odom_in);
|
mavlink_msg_odometry_decode(msg, &odom_in);
|
||||||
|
|
||||||
|
odometry_sent_counter++;
|
||||||
|
|
||||||
// fill vehicle_odometry from Mavlink ODOMETRY
|
// fill vehicle_odometry from Mavlink ODOMETRY
|
||||||
vehicle_odometry_s odom{};
|
vehicle_odometry_s odom{};
|
||||||
uint64_t timestamp = hrt_absolute_time();
|
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.reset_counter = odom_in.reset_counter;
|
||||||
odom.quality = odom_in.quality;
|
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) {
|
switch (odom_in.estimator_type) {
|
||||||
case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
|
case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
|
||||||
case MAV_ESTIMATOR_TYPE_NAIVE:
|
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 = mode_flag_custom;
|
||||||
msg->mode |= (armed) ? mode_flag_armed : 0;
|
msg->mode |= (armed) ? mode_flag_armed : 0;
|
||||||
msg->flags = 0;
|
msg->flags = 0;
|
||||||
|
|
||||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
|
||||||
msg->flags |= 1;
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int openPort(const char *dev, speed_t speed)
|
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);
|
_uart_fd = qurt_uart_open(dev, speed);
|
||||||
PX4_DEBUG("qurt_uart_opened");
|
|
||||||
|
if (_debug) { PX4_INFO("qurt_uart_opened"); }
|
||||||
|
|
||||||
if (_uart_fd < 0) {
|
if (_uart_fd < 0) {
|
||||||
PX4_ERR("Error opening port: %s (%i)", dev, errno);
|
PX4_ERR("Error opening port: %s (%i)", dev, errno);
|
||||||
|
@ -840,25 +1009,50 @@ int stop()
|
||||||
|
|
||||||
void usage()
|
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("Running: %s", _is_running ? "yes" : "no");
|
||||||
PX4_INFO("Status of IMU_Data counter: %i", imu_counter);
|
PX4_INFO("HIL Sensor received: %i", hil_sensor_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("IMU updates: %i", imu_counter);
|
||||||
PX4_INFO("Value of current gyro x, y, z data: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
PX4_INFO("\tCurrent accel x, y, z: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
|
||||||
PX4_INFO("Value of HIL_Sensor counter: %i", hil_sensor_counter);
|
PX4_INFO("\tCurrent gyro x, y, z: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
||||||
PX4_INFO("Value of Heartbeat counter: %i", heartbeat_counter);
|
PX4_INFO("Magnetometer sent: %i", mag_counter);
|
||||||
PX4_INFO("Value of Vision data counter: %i", vision_msg_counter);
|
PX4_INFO("Barometer sent: %i", baro_counter);
|
||||||
PX4_INFO("Value of GPS Data counter: %i", gps_counter);
|
PX4_INFO("Heartbeat received: %i, sent: %i", heartbeat_received_counter, heartbeat_sent_counter);
|
||||||
return 0;
|
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;
|
void
|
||||||
uint64_t first_sensor_report_timestamp = 0;
|
clear_status_counters()
|
||||||
uint64_t last_sensor_report_timestamp = 0;
|
{
|
||||||
|
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
|
void
|
||||||
handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
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);
|
_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.error_count = 0;
|
||||||
sensor_baro.timestamp = hrt_absolute_time();
|
sensor_baro.timestamp = hrt_absolute_time();
|
||||||
_sensor_baro_pub.publish(sensor_baro);
|
_sensor_baro_pub.publish(sensor_baro);
|
||||||
}
|
|
||||||
|
|
||||||
// differential pressure
|
baro_counter++;
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// battery status
|
// battery status
|
||||||
|
@ -970,7 +1157,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
||||||
|
|
||||||
_battery_pub.publish(hil_battery_status);
|
_battery_pub.publish(hil_battery_status);
|
||||||
}
|
}
|
||||||
hil_sensor_counter++;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -989,15 +1175,41 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
||||||
|
|
||||||
gps.device_id = device_id.devid;
|
gps.device_id = device_id.devid;
|
||||||
|
|
||||||
gps.latitude_deg = hil_gps.lat;
|
gps.latitude_deg = hil_gps.lat * 1e-7;
|
||||||
gps.longitude_deg = hil_gps.lon;
|
gps.longitude_deg = hil_gps.lon * 1e-7;
|
||||||
gps.altitude_msl_m = hil_gps.alt;
|
gps.altitude_msl_m = hil_gps.alt * 1e-3;
|
||||||
gps.altitude_ellipsoid_m = hil_gps.alt;
|
gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3;
|
||||||
|
|
||||||
gps.s_variance_m_s = 0.25f;
|
gps.s_variance_m_s = 0.25f;
|
||||||
gps.c_variance_rad = 0.5f;
|
gps.c_variance_rad = 0.5f;
|
||||||
|
|
||||||
|
gps.satellites_used = hil_gps.satellites_visible;
|
||||||
gps.fix_type = hil_gps.fix_type;
|
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.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
|
||||||
gps.epv = (float)hil_gps.epv * 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.timestamp_time_relative = 0;
|
||||||
gps.time_utc_usec = hil_gps.time_usec;
|
gps.time_utc_usec = hil_gps.time_usec;
|
||||||
|
|
||||||
gps.satellites_used = hil_gps.satellites_visible;
|
|
||||||
|
|
||||||
gps.heading = NAN;
|
gps.heading = NAN;
|
||||||
gps.heading_offset = NAN;
|
gps.heading_offset = NAN;
|
||||||
|
@ -1029,10 +1240,51 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
||||||
gps.timestamp = hrt_absolute_time();
|
gps.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
_sensor_gps_pub.publish(gps);
|
_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 dsp_hitl_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
int myoptind = 1;
|
int myoptind = 1;
|
||||||
|
@ -1044,20 +1296,47 @@ int dsp_hitl_main(int argc, char *argv[])
|
||||||
|
|
||||||
const char *verb = argv[myoptind];
|
const char *verb = argv[myoptind];
|
||||||
|
|
||||||
|
|
||||||
if (!strcmp(verb, "start")) {
|
if (!strcmp(verb, "start")) {
|
||||||
return dsp_hitl::start(argc - 1, argv + 1);
|
return dsp_hitl::start(argc - 1, argv + 1);
|
||||||
}
|
|
||||||
|
|
||||||
else if (!strcmp(verb, "stop")) {
|
} else if (!strcmp(verb, "stop")) {
|
||||||
return dsp_hitl::stop();
|
return dsp_hitl::stop();
|
||||||
}
|
|
||||||
|
|
||||||
else if (!strcmp(verb, "status")) {
|
} else if (!strcmp(verb, "status")) {
|
||||||
return dsp_hitl::get_status();
|
dsp_hitl::print_status();
|
||||||
}
|
return 0;
|
||||||
|
|
||||||
else {
|
} 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
} else {
|
||||||
dsp_hitl::usage();
|
dsp_hitl::usage();
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
|
@ -82,6 +82,8 @@ load_mon start
|
||||||
microdds_client start -t udp -h 127.0.0.1 -p 8888
|
microdds_client start -t udp -h 127.0.0.1 -p 8888
|
||||||
|
|
||||||
qshell pwm_out_sim start -m hil
|
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
|
qshell dsp_hitl start -g -m
|
||||||
|
|
||||||
# start the onboard fast link to connect to voxl-mavlink-server
|
# start the onboard fast link to connect to voxl-mavlink-server
|
||||||
|
|
Loading…
Reference in New Issue