Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: added USD1 CAN driver

Browse Source 
also known as uLanding CAN
This commit is contained in:
Andrew Tridgell 2021-01-01 11:10:39 +11:00
parent 943918d511
commit 66c05bef76
5 changed files with 75 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -47,6 +47,7 @@
#include "AP_RangeFinder_LeddarVu8.h"
#include "AP_RangeFinder_SITL.h"
#include "AP_RangeFinder_MSP.h"
#include "AP_RangeFinder_USD1_CAN.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Logger/AP_Logger.h>
@ -564,7 +565,13 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
#endif // HAL_MSP_RANGEFINDER_ENABLED
break;
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
case Type::USD1_CAN:
_add_backend(new AP_RangeFinder_USD1_CAN(state[instance], params[instance]), instance);
break;
#endif
case Type::NONE:
default:
break;
}

1
libraries/AP_RangeFinder/AP_RangeFinder.h
View File

@ -87,6 +87,7 @@ public:
HC_SR04 = 30,
GYUS42v2 = 31,
MSP = 32,
USD1_CAN = 33,
SITL = 100,
};

2
libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
View File

@ -6,7 +6,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
// @Param: TYPE
// @DisplayName: Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,100:SITL
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,100:SITL
// @User: Standard
AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),

39
libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.cpp Normal file
View File

@ -0,0 +1,39 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_RangeFinder_USD1_CAN.h"
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
/*
constructor
*/
AP_RangeFinder_USD1_CAN::AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
CANSensor("USD1", AP_CANManager::Driver_Type_USD1),
AP_RangeFinder_Backend(_state, _params)
{
}
// update state
void AP_RangeFinder_USD1_CAN::update(void)
{
WITH_SEMAPHORE(_sem);
if ((AP_HAL::millis() - _last_reading_ms) > 500) {
// if data is older than 500ms, report NoData
set_status(RangeFinder::Status::NoData);
} else if (new_data) {
state.distance_cm = _distance_cm;
state.last_reading_ms = _last_reading_ms;
update_status();
new_data = false;
}
}
// handler for incoming frames
void AP_RangeFinder_USD1_CAN::handle_frame(AP_HAL::CANFrame &frame)
{
WITH_SEMAPHORE(_sem);
_distance_cm = (frame.data[0]<<8) | frame.data[1];
_last_reading_ms = AP_HAL::millis();
new_data = true;
}
#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS

27
libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h Normal file
View File

@ -0,0 +1,27 @@
#pragma once
#include "AP_RangeFinder_Backend.h"
#include <AP_CANManager/AP_CANSensor.h>
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
class AP_RangeFinder_USD1_CAN : public CANSensor, public AP_RangeFinder_Backend {
public:
AP_RangeFinder_USD1_CAN(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
void update() override;
// handler for incoming frames
void handle_frame(AP_HAL::CANFrame &frame) override;
protected:
virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_RADAR;
}
private:
bool new_data;
uint16_t _distance_cm;
uint32_t _last_reading_ms;
};
#endif //HAL_MAX_CAN_PROTOCOL_DRIVERS