From 853bfbf64d4a8fd663de36c1a06625808f7c9ab4 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Thu, 28 Mar 2024 13:21:36 +0000 Subject: [PATCH] AP_Periph: Add support for up to two rangefinders --- Tools/AP_Periph/AP_Periph.cpp | 24 ++++-- Tools/AP_Periph/AP_Periph.h | 17 +++- Tools/AP_Periph/Parameters.cpp | 35 +++++++- Tools/AP_Periph/Parameters.h | 13 ++- Tools/AP_Periph/rangefinder.cpp | 134 ++++++++++++++++++------------ Tools/AP_Periph/serial_tunnel.cpp | 2 +- 6 files changed, 155 insertions(+), 70 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index f02037e5a2..b9c86c1a64 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -227,15 +227,20 @@ void AP_Periph_FW::init() #endif #ifdef HAL_PERIPH_ENABLE_RANGEFINDER - if (rangefinder.get_type(0) != RangeFinder::Type::NONE) { - if (g.rangefinder_port >= 0) { + bool have_rangefinder = false; + for (uint8_t i=0; i= 0)) { // init uart for serial rangefinders - auto *uart = hal.serial(g.rangefinder_port); + auto *uart = hal.serial(g.rangefinder_port[i]); if (uart != nullptr) { - uart->begin(g.rangefinder_baud); - serial_manager.set_protocol_and_baud(g.rangefinder_port, AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud); + uart->begin(g.rangefinder_baud[i]); + serial_manager.set_protocol_and_baud(g.rangefinder_port[i], AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud[i]); + have_rangefinder = true; } } + } + if (have_rangefinder) { + // Can only call rangefinder init once, subsequent inits are blocked rangefinder.init(ROTATION_NONE); } #endif @@ -419,7 +424,14 @@ void AP_Periph_FW::update() hal.serial(0)->printf("BARO H=%u P=%.2f T=%.2f\n", baro.healthy(), baro.get_pressure(), baro.get_temperature()); #endif #ifdef HAL_PERIPH_ENABLE_RANGEFINDER - hal.serial(0)->printf("RNG %u %ucm\n", rangefinder.num_sensors(), rangefinder.distance_cm_orient(ROTATION_NONE)); + hal.serial(0)->printf("Num RNG sens %u\n", rangefinder.num_sensors()); + for (uint8_t i=0; iprintf("RNG %u %ucm\n", i, uint16_t(backend->distance()*100)); + } #endif hal.scheduler->delay(1); #endif diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 95d32af793..eff5ccd6b0 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -168,7 +169,9 @@ public: void send_relposheading_msg(); void can_baro_update(); void can_airspeed_update(); +#ifdef HAL_PERIPH_ENABLE_RANGEFINDER void can_rangefinder_update(); +#endif void can_battery_update(); void can_battery_send_cells(uint8_t instance); void can_proximity_update(); @@ -279,7 +282,8 @@ public: #ifdef HAL_PERIPH_ENABLE_RANGEFINDER RangeFinder rangefinder; - uint32_t last_sample_ms; + uint32_t last_rangefinder_update_ms; + uint32_t last_rangefinder_sample_ms[RANGEFINDER_MAX_INSTANCES]; #endif #ifdef HAL_PERIPH_ENABLE_PROXIMITY @@ -451,7 +455,16 @@ public: bool debug_option_is_set(const DebugOptions option) const { return (uint8_t(g.debug.get()) & (1U< 1 + // @Param: RNGFND2_BAUDRATE + // @DisplayName: Rangefinder serial baudrate + // @Description: Rangefinder serial baudrate. + // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,230:230400,256:256000,460:460800,500:500000,921:921600,1500:1500000 + // @Increment: 1 + // @User: Standard + // @RebootRequired: True + GARRAY(rangefinder_baud, 1, "RNGFND2_BAUDRATE", HAL_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT), + + // @Param: RNGFND2_PORT + // @DisplayName: Rangefinder Serial Port + // @Description: This is the serial port number where SERIALx_PROTOCOL will be set to Rangefinder. + // @Range: 0 10 + // @Increment: 1 + // @User: Advanced + // @RebootRequired: True + GARRAY(rangefinder_port, 1, "RNGFND2_PORT", -1), +#endif // @Param: RNGFND_MAX_RATE // @DisplayName: Rangefinder max rate @@ -661,6 +685,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GSCALAR(temperature_msg_rate, "TEMP_MSG_RATE", 0), #endif + // @Param: OPTIONS + // @DisplayName: AP Periph Options + // @Description: Bitmask of AP Periph Options + // @Bitmask: 0: Enable continuous sensor probe + // @User: Standard + GSCALAR(options, "OPTIONS", AP_PERIPH_PROBE_CONTINUOUS), + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 94ac49b39a..9d51c97cb0 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -25,7 +25,7 @@ public: k_param_airspeed, k_param_rangefinder, k_param_flash_bootloader, - k_param_rangefinder_baud, + k_param_rangefinder_baud0, k_param_adsb_baudrate, k_param_hardpoint_id, k_param_hardpoint_rate, @@ -36,7 +36,7 @@ public: k_param_serial_number, k_param_adsb_port, k_param_servo_channels, - k_param_rangefinder_port, + k_param_rangefinder_port0, k_param_gps_port, k_param_msp_port, k_param_notify, @@ -91,6 +91,9 @@ public: k_param_serial_options, k_param_relay, k_param_temperature_msg_rate, + k_param_rangefinder_baud1, + k_param_rangefinder_port1, + k_param_options, }; AP_Int16 format_version; @@ -119,8 +122,8 @@ public: #endif #ifdef HAL_PERIPH_ENABLE_RANGEFINDER - AP_Int32 rangefinder_baud; - AP_Int8 rangefinder_port; + AP_Int32 rangefinder_baud[RANGEFINDER_MAX_INSTANCES]; + AP_Int8 rangefinder_port[RANGEFINDER_MAX_INSTANCES]; AP_Int16 rangefinder_max_rate; #endif @@ -216,6 +219,8 @@ public: static constexpr uint8_t can_fdmode = 0; #endif + AP_Int32 options; + AP_Int8 can_terminate[HAL_NUM_CAN_IFACES]; AP_Int8 node_stats; diff --git a/Tools/AP_Periph/rangefinder.cpp b/Tools/AP_Periph/rangefinder.cpp index 43ad6d65e5..fc74399f32 100644 --- a/Tools/AP_Periph/rangefinder.cpp +++ b/Tools/AP_Periph/rangefinder.cpp @@ -12,6 +12,8 @@ #define AP_PERIPH_PROBE_CONTINUOUS 0 #endif +extern const AP_HAL::HAL &hal; + /* update CAN rangefinder */ @@ -21,7 +23,8 @@ void AP_Periph_FW::can_rangefinder_update(void) return; } #if AP_PERIPH_PROBE_CONTINUOUS - if (rangefinder.num_sensors() == 0) { + // We only allow continuous probing for rangefinders while vehicle is disarmed. Probing is currently inefficient and leads to longer loop times. + if ((rangefinder.num_sensors() == 0) && !hal.util->get_soft_armed() && option_is_set(PeriphOptions::PROBE_CONTINUOUS)) { uint32_t now = AP_HAL::millis(); static uint32_t last_probe_ms; if (now - last_probe_ms >= 1000) { @@ -31,67 +34,88 @@ void AP_Periph_FW::can_rangefinder_update(void) } #endif uint32_t now = AP_HAL::millis(); - static uint32_t last_update_ms; if (g.rangefinder_max_rate > 0 && - now - last_update_ms < uint32_t(1000/g.rangefinder_max_rate)) { + now - last_rangefinder_update_ms < uint32_t(1000/g.rangefinder_max_rate)) { // limit to max rate return; } - last_update_ms = now; + + // update all rangefinder instances rangefinder.update(); - RangeFinder::Status status = rangefinder.status_orient(ROTATION_NONE); - if (status <= RangeFinder::Status::NoData) { - // don't send any data - return; - } - const uint32_t sample_ms = rangefinder.last_reading_ms(ROTATION_NONE); - if (last_sample_ms == sample_ms) { - return; - } - last_sample_ms = sample_ms; - uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE); - uavcan_equipment_range_sensor_Measurement pkt {}; - pkt.sensor_id = rangefinder.get_address(0); - switch (status) { - case RangeFinder::Status::OutOfRangeLow: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE; - break; - case RangeFinder::Status::OutOfRangeHigh: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR; - break; - case RangeFinder::Status::Good: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE; - break; - default: - pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED; - break; + // cycle through each rangefinder instance to find one to send + // equipment.range_sensor only uses 3 CAN frames so we just send all available sensor measurements. + for (uint8_t i = 0; i <= rangefinder.num_sensors(); i++) { + + if (rangefinder.get_type(i) == RangeFinder::Type::NONE) { + continue; + } + + AP_RangeFinder_Backend *backend = rangefinder.get_backend(i); + if (backend == nullptr) { + continue; + } + + RangeFinder::Status status = backend->status(); + if (status <= RangeFinder::Status::NoData) { + // don't send any data for this instance + continue; + } + + const uint32_t sample_ms = backend->last_reading_ms(); + if (last_rangefinder_sample_ms[i] == sample_ms) { + // don't same the same reading again + continue; + } + last_rangefinder_sample_ms[i] = sample_ms; + + uavcan_equipment_range_sensor_Measurement pkt {}; + pkt.sensor_id = rangefinder.get_address(i); + + switch (status) { + case RangeFinder::Status::OutOfRangeLow: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE; + break; + case RangeFinder::Status::OutOfRangeHigh: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR; + break; + case RangeFinder::Status::Good: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE; + break; + default: + pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_UNDEFINED; + break; + } + + switch (backend->get_mav_distance_sensor_type()) { + case MAV_DISTANCE_SENSOR_LASER: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR; + break; + case MAV_DISTANCE_SENSOR_ULTRASOUND: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR; + break; + case MAV_DISTANCE_SENSOR_RADAR: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR; + break; + default: + pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_UNDEFINED; + break; + } + + float dist_m = backend->distance(); + pkt.range = dist_m; + + uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {}; + uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, + UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + + last_rangefinder_update_ms = now; } - switch (rangefinder.get_mav_distance_sensor_type_orient(ROTATION_NONE)) { - case MAV_DISTANCE_SENSOR_LASER: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR; - break; - case MAV_DISTANCE_SENSOR_ULTRASOUND: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR; - break; - case MAV_DISTANCE_SENSOR_RADAR: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR; - break; - default: - pkt.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_UNDEFINED; - break; - } - - pkt.range = dist_cm * 0.01; - - uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout()); - - canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, - UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); } #endif // HAL_PERIPH_ENABLE_RANGEFINDER diff --git a/Tools/AP_Periph/serial_tunnel.cpp b/Tools/AP_Periph/serial_tunnel.cpp index cdba23b41c..6a99126980 100644 --- a/Tools/AP_Periph/serial_tunnel.cpp +++ b/Tools/AP_Periph/serial_tunnel.cpp @@ -50,7 +50,7 @@ int8_t AP_Periph_FW::get_default_tunnel_serial_port(void) const #endif #ifdef HAL_PERIPH_ENABLE_RANGEFINDER if (uart_num == -1) { - uart_num = g.rangefinder_port; + uart_num = g.rangefinder_port[0]; } #endif #ifdef HAL_PERIPH_ENABLE_ADSB