diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp index 37abacd666..b26f565082 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp @@ -62,6 +62,8 @@ #include #include #include +#include +#include #include #include @@ -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_pub{ORB_ID(battery_status)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; -uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; +uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; +uORB::Publication _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(dist_sensor.min_distance) * 1e-2f; /* cm to m */ + ds.max_distance = static_cast(dist_sensor.max_distance) * 1e-2f; /* cm to m */ + ds.current_distance = static_cast(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 "); + 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")) { - return dsp_hitl::get_status(); - } + } else if (!strcmp(verb, "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(); return 1; } diff --git a/boards/modalai/voxl2/target/voxl-px4-hitl-start b/boards/modalai/voxl2/target/voxl-px4-hitl-start index d56f489e29..eff2059ce7 100755 --- a/boards/modalai/voxl2/target/voxl-px4-hitl-start +++ b/boards/modalai/voxl2/target/voxl-px4-hitl-start @@ -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