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

AP_RangeFinder: BlueRobotics Ping driver

This commit is contained in:
Randy Mackay 2019-03-28 21:43:23 +09:00
parent 65966ac957
commit 5e9aebd678
5 changed files with 372 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

292
libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp Normal file
View File

@ -0,0 +1,292 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <GCS_MAVLink/GCS.h>
#include "AP_RangeFinder_BLPing.h"
#define BLPING_TIMEOUT_MS 500 // sensor timeout after 0.5 sec
#define BLPING_INIT_RATE_MS 1000 // initialise sensor at no more than 1hz
#define BLPING_FRAME_HEADER1 0x42 // header first byte ('B')
#define BLPING_FRAME_HEADER2 0x52 // header second byte ('R')
#define BLPING_SRC_ID 0 // vehicle's source id
#define BLPING_DEST_ID 1 // sensor's id
#define BLPING_MSGID_ACK 1
#define BLPING_MSGID_NACK 2
#define BLPING_MSGID_SET_PING_INTERVAL 1004
#define BLPING_MSGID_GET_DEVICE_ID 1201
#define BLDPIN_MSGID_DISTANCE_SIMPLE 1211
#define BLPING_MSGID_CONTINUOUS_START 1400
// Protocol implemented by this sensor can be found here: https://github.com/bluerobotics/ping-protocol
//
// Byte Type Name Description
// --------------------------------------------------------------------------------------------------------------
// 0 uint8_t start1 'B'
// 1 uint8_t start2 'R'
// 2-3 uint16_t payload_length number of bytes in payload (low byte, high byte)
// 4-5 uint16_t message id message id (low byte, high byte)
// 6 uint8_t src_device_id id of device sending the message
// 7 uint8_t dst_device_id id of device of the intended recipient
// 8-n uint8_t[] payload message payload
// (n+1)-(n+2) uint16_t checksum the sum of all the non-checksum bytes in the message (low byte, high byte)
/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_BLPing::AP_RangeFinder_BLPing(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state, _params)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance));
}
}
// detect if a serial port has been setup to accept rangefinder input
bool AP_RangeFinder_BLPing::detect(AP_SerialManager &serial_manager, uint8_t serial_instance)
{
return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr;
}
/*
update the state of the sensor
*/
void AP_RangeFinder_BLPing::update(void)
{
if (uart == nullptr) {
return;
}
const uint32_t now = AP_HAL::millis();
if (get_reading(state.distance_cm)) {
// update range_valid state based on distance measured
state.last_reading_ms = now;
update_status();
} else if (now - state.last_reading_ms > BLPING_TIMEOUT_MS) {
set_status(RangeFinder::RangeFinder_NoData);
// initialise sensor if no distances recently
if (now - last_init_ms > BLPING_INIT_RATE_MS) {
last_init_ms = now;
init_sensor();
}
}
}
void AP_RangeFinder_BLPing::init_sensor()
{
// request distance from sensor
send_message(BLDPIN_MSGID_DISTANCE_SIMPLE, nullptr, 0);
}
// send message to sensor
void AP_RangeFinder_BLPing::send_message(uint16_t msgid, const uint8_t *payload, uint16_t payload_len)
{
if (uart == nullptr) {
return;
}
// check for sufficient space in outgoing buffer
if (uart->txspace() < payload_len + 10U) {
return;
}
// write header
uart->write((uint8_t)BLPING_FRAME_HEADER1);
uart->write((uint8_t)BLPING_FRAME_HEADER2);
uint16_t crc = BLPING_FRAME_HEADER1 + BLPING_FRAME_HEADER2;
// write payload length
uart->write(LOWBYTE(payload_len));
uart->write(HIGHBYTE(payload_len));
crc += LOWBYTE(payload_len) + HIGHBYTE(payload_len);
// msgid
uart->write(LOWBYTE(msgid));
uart->write(HIGHBYTE(msgid));
crc += LOWBYTE(msgid) + HIGHBYTE(msgid);
// src dev id
uart->write((uint8_t)BLPING_SRC_ID);
crc += BLPING_SRC_ID;
// destination dev id
uart->write((uint8_t)BLPING_DEST_ID);
crc += BLPING_DEST_ID;
// payload
if (payload != nullptr) {
for (uint16_t i = 0; i<payload_len; i++) {
uart->write(payload[i]);
crc += payload[i];
}
}
// checksum
uart->write(LOWBYTE(crc));
uart->write(HIGHBYTE(crc));
}
// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
bool AP_RangeFinder_BLPing::get_reading(uint16_t &reading_cm)
{
if (uart == nullptr) {
return false;
}
float sum_cm = 0;
uint16_t count = 0;
// read any available lines from the lidar
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
const int16_t b = uart->read();
if (b < 0) {
break;
}
if (parse_byte(b)) {
count++;
sum_cm += distance_cm;
}
}
if (count > 0) {
// return average distance of readings
reading_cm = sum_cm / count;
// request another distance
send_message(BLDPIN_MSGID_DISTANCE_SIMPLE, nullptr, 0);
return true;
}
// no readings so return false
return false;
}
// process one byte received on serial port
// returns true if a distance message has been successfully parsed
// state is stored in msg structure
bool AP_RangeFinder_BLPing::parse_byte(uint8_t b)
{
bool got_distance = false;
// process byte depending upon current state
switch (msg.state) {
case ParseState::HEADER1:
if (b == BLPING_FRAME_HEADER1) {
msg.crc_expected = BLPING_FRAME_HEADER1;
msg.state = ParseState::HEADER2;
}
break;
case ParseState::HEADER2:
if (b == BLPING_FRAME_HEADER2) {
msg.crc_expected += BLPING_FRAME_HEADER2;
msg.state = ParseState::LEN_L;
} else {
msg.state = ParseState::HEADER1;
}
break;
case ParseState::LEN_L:
msg.payload_len = b;
msg.crc_expected += b;
msg.state = ParseState::LEN_H;
break;
case ParseState::LEN_H:
msg.payload_len |= ((uint16_t)b << 8);
msg.payload_recv = 0;
msg.crc_expected += b;
msg.state = ParseState::MSG_ID_L;
break;
case ParseState::MSG_ID_L:
msg.msgid = b;
msg.crc_expected += b;
msg.state = ParseState::MSG_ID_H;
break;
case ParseState::MSG_ID_H:
msg.msgid |= ((uint16_t)b << 8);
msg.crc_expected += b;
msg.state = ParseState::SRC_ID;
break;
case ParseState::SRC_ID:
msg.crc_expected += b;
msg.state = ParseState::DST_ID;
break;
case ParseState::DST_ID:
msg.crc_expected += b;
msg.state = ParseState::PAYLOAD;
break;
case ParseState::PAYLOAD:
if (msg.payload_recv < msg.payload_len) {
if (msg.payload_recv < ARRAY_SIZE(msg.payload)) {
msg.payload[msg.payload_recv] = b;
}
msg.payload_recv++;
msg.crc_expected += b;
}
if (msg.payload_recv == msg.payload_len) {
msg.state = ParseState::CRC_L;
}
break;
case ParseState::CRC_L:
msg.crc = b;
msg.state = ParseState::CRC_H;
break;
case ParseState::CRC_H:
msg.crc |= ((uint16_t)b << 8);
msg.state = ParseState::HEADER1;
if (msg.crc_expected == msg.crc) {
// process payload
switch (msg.msgid) {
case BLPING_MSGID_ACK:
case BLPING_MSGID_NACK:
// ignore
break;
case BLDPIN_MSGID_DISTANCE_SIMPLE:
const uint32_t dist_mm = (uint32_t)msg.payload[0] | (uint32_t)msg.payload[1] << 8 | (uint32_t)msg.payload[2] << 16 | (uint32_t)msg.payload[3] << 24;
distance_cm = dist_mm / 10;
got_distance = true;
break;
}
}
break;
}
return got_distance;
}

72
libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h Normal file
View File

@ -0,0 +1,72 @@
#pragma once
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
class AP_RangeFinder_BLPing : public AP_RangeFinder_Backend
{
public:
// constructor
AP_RangeFinder_BLPing(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
AP_SerialManager &serial_manager,
uint8_t serial_instance);
// static detection function
static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance);
// update state
void update(void) override;
protected:
virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_ULTRASOUND;
}
private:
void init_sensor();
// send message to sensor
void send_message(uint16_t msgid, const uint8_t *payload, uint16_t payload_len);
// read a distance from the sensor
bool get_reading(uint16_t &reading_cm);
// process one byte received on serial port
// returns true if a distance message has been successfully parsed
// state is stored in msg structure
bool parse_byte(uint8_t b);
enum class ParseState {
HEADER1 = 0,
HEADER2,
LEN_L,
LEN_H,
MSG_ID_L,
MSG_ID_H,
SRC_ID,
DST_ID,
PAYLOAD,
CRC_L,
CRC_H
};
AP_HAL::UARTDriver *uart;
uint32_t last_init_ms; // system time that sensor was last initialised
uint16_t distance_cm; // latest distance
// structure holding latest message contents
struct {
ParseState state; // state of incoming message processing
uint8_t payload[20]; // payload
uint16_t payload_len; // latest message payload length
uint16_t msgid; // latest message's message id
uint16_t payload_recv; // number of message's payload bytes received so far
uint16_t crc; // latest message's crc
uint16_t crc_expected; // latest message's expected crc
} msg;
};

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:LidarLiteV2-I2C,5:PX4-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,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLightV3HP,22:PWM
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-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,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing
// @User: Standard
AP_GROUPINFO("TYPE", 1, AP_RangeFinder_Params, type, 0),

6
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -36,6 +36,7 @@
#include "AP_RangeFinder_Wasp.h"
#include "AP_RangeFinder_Benewake.h"
#include "AP_RangeFinder_PWM.h"
#include "AP_RangeFinder_BLPing.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
@ -493,6 +494,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
drivers[instance] = new AP_RangeFinder_PWM(state[instance], params[instance], estimated_terrain_height);
}
break;
case RangeFinder_TYPE_BLPing:
if (AP_RangeFinder_BLPing::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_BLPing(state[instance], params[instance], serial_manager, serial_instance++);
}
break;
default:
break;
}

1
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -68,6 +68,7 @@ public:
RangeFinder_TYPE_BenewakeTFmini = 20,
RangeFinder_TYPE_PLI2CV3HP = 21,
RangeFinder_TYPE_PWM = 22,
RangeFinder_TYPE_BLPing = 23,
};
enum RangeFinder_Function {