Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: support rangefinder CAN nodes

This commit is contained in:
Andrew Tridgell 2019-10-19 15:36:14 +11:00
parent ca1bd8be3c
commit 55b623d176
6 changed files with 104 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/AP_Periph/AP_Periph.cpp
View File

@ -82,6 +82,10 @@ void AP_Periph_FW::init()
#ifdef HAL_PERIPH_ENABLE_AIRSPEED
airspeed.init();
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
rangefinder.init(ROTATION_NONE);
#endif
start_ms = AP_HAL::millis();
}

6
Tools/AP_Periph/AP_Periph.h
View File

@ -4,6 +4,7 @@
#include <AP_Compass/AP_Compass.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_NCP5623_LED)
#define AP_PERIPH_HAVE_LED
@ -26,6 +27,7 @@ public:
void can_gps_update();
void can_baro_update();
void can_airspeed_update();
void can_rangefinder_update();
void load_parameters();
@ -56,6 +58,10 @@ public:
#ifdef HAL_PERIPH_ENABLE_AIRSPEED
AP_Airspeed airspeed;
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
RangeFinder rangefinder;
#endif
// setup the var_info table
AP_Param param_loader{var_info};

15
Tools/AP_Periph/Parameters.cpp
View File

@ -37,20 +37,20 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
#ifdef HAL_PERIPH_ENABLE_GPS
// GPS driver
// @Group: GPS_
// @Path: ../libraries/AP_GPS/AP_GPS.cpp
// @Path: ../../libraries/AP_GPS/AP_GPS.cpp
GOBJECT(gps, "GPS_", AP_GPS),
#endif
#ifdef HAL_PERIPH_ENABLE_MAG
// @Group: COMPASS_
// @Path: ../libraries/AP_Compass/AP_Compass.cpp
// @Path: ../../libraries/AP_Compass/AP_Compass.cpp
GOBJECT(compass, "COMPASS_", Compass),
#endif
#ifdef HAL_PERIPH_ENABLE_BARO
// Baro driver
// @Group: BARO_
// @Path: ../libraries/AP_Baro/AP_Baro.cpp
// @Path: ../../libraries/AP_Baro/AP_Baro.cpp
GOBJECT(baro, "BARO_", AP_Baro),
#endif
@ -61,9 +61,16 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
#ifdef HAL_PERIPH_ENABLE_AIRSPEED
// Airspeed driver
// @Group: ARSP
// @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp
// @Path: ../../libraries/AP_Airspeed/AP_Airspeed.cpp
GOBJECT(airspeed, "ARSP", AP_Airspeed),
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
// Rangefinder driver
// @Group: RNGFND
// @Path: ../../libraries/AP_RangeFinder/Rangefinder.cpp
GOBJECT(rangefinder, "RNGFND", RangeFinder),
#endif
AP_VAREND
};

1
Tools/AP_Periph/Parameters.h
View File

@ -20,6 +20,7 @@ public:
k_param_buzz_volume,
k_param_led_brightness,
k_param_airspeed,
k_param_rangefinder,
};
AP_Int16 format_version;

81
Tools/AP_Periph/can.cpp
View File

@ -35,6 +35,7 @@
#include <uavcan/equipment/air_data/RawAirData.h>
#include <uavcan/equipment/indication/BeepCommand.h>
#include <uavcan/equipment/indication/LightsCommand.h>
#include <uavcan/equipment/range_sensor/Measurement.h>
#include <ardupilot/indication/SafetyState.h>
#include <ardupilot/indication/Button.h>
#include <ardupilot/equipment/trafficmonitor/TrafficReport.h>
@ -265,6 +266,9 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr
#endif
#ifdef HAL_PERIPH_ENABLE_AIRSPEED
AP_Param::setup_object_defaults(&periph.airspeed, periph.airspeed.var_info);
#endif
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
AP_Param::setup_object_defaults(&periph.rangefinder, periph.rangefinder.var_info);
#endif
}
@ -899,6 +903,7 @@ void AP_Periph_FW::can_update()
can_gps_update();
can_baro_update();
can_airspeed_update();
can_rangefinder_update();
#ifdef HAL_PERIPH_ENABLE_BUZZER
can_buzzer_update();
#endif
@ -1189,6 +1194,82 @@ void AP_Periph_FW::can_airspeed_update(void)
#endif // HAL_PERIPH_ENABLE_AIRSPEED
}
/*
update CAN rangefinder
*/
void AP_Periph_FW::can_rangefinder_update(void)
{
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
if (rangefinder.num_sensors() == 0) {
uint32_t now = AP_HAL::millis();
static uint32_t last_probe_ms;
if (now - last_probe_ms >= 1000) {
last_probe_ms = now;
rangefinder.init(ROTATION_NONE);
}
}
uint32_t now = AP_HAL::millis();
static uint32_t last_update_ms;
if (now - last_update_ms < 20) {
// max 50Hz data
return;
}
last_update_ms = now;
rangefinder.update();
RangeFinder::RangeFinder_Status status = rangefinder.status_orient(ROTATION_NONE);
if (status <= RangeFinder::RangeFinder_NoData) {
// don't send any data
return;
}
uint16_t dist_cm = rangefinder.distance_cm_orient(ROTATION_NONE);
uavcan_equipment_range_sensor_Measurement pkt {};
switch (status) {
case RangeFinder::RangeFinder_OutOfRangeLow:
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE;
break;
case RangeFinder::RangeFinder_OutOfRangeHigh:
pkt.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR;
break;
case RangeFinder::RangeFinder_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 (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;
fix_float16(pkt.range);
uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE];
uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE,
UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
#endif // HAL_PERIPH_ENABLE_RANGEFINDER
}
#ifdef HAL_PERIPH_ENABLE_ADSB
/*
map an ADSB_VEHICLE MAVLink message to a UAVCAN TrafficReport message

1
Tools/AP_Periph/wscript
View File

@ -33,6 +33,7 @@ def build(bld):
'AP_InternalError',
'GCS_MAVLink',
'AP_Airspeed',
'AP_RangeFinder',
],
exclude_src=[
'libraries/AP_HAL_ChibiOS/Storage.cpp'