zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

add NanoRadar MR72,test ok

master
z 5 years ago
parent
81a930b1be
commit
a638909303
6 changed files with 527 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 139
      libraries/AP_Proximity/AP_Proximity_NanaRadar_MR72.cpp
  2. 42
      libraries/AP_Proximity/AP_Proximity_NanaRadar_MR72.h
  3. 161
      libraries/AP_RangeFinder/AP_RangeFinder_NanoRadar_MR72.cpp
  4. 39
      libraries/AP_RangeFinder/AP_RangeFinder_NanoRadar_MR72.h
  5. 110
      libraries/AP_RangeFinder/AP_RangeFinder_insighticaSerial.cpp
  6. 36
      libraries/AP_RangeFinder/AP_RangeFinder_insighticaSerial.h

139
libraries/AP_Proximity/AP_Proximity_NanaRadar_MR72.cpp
Unescape View File

@ -0,0 +1,139 @@ @@ -0,0 +1,139 @@
/*
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_Proximity_NanaRadar_MR72.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Math/crc.h>
#include <ctype.h>
#include <stdio.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
/*
The constructor also initialises the proximity sensor. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the proximity sensor
*/
AP_Proximity_NanaRadar_MR72::AP_Proximity_NanaRadar_MR72(
AP_Proximity &_frontend,
AP_Proximity::Proximity_State &_state) :
AP_Proximity_Backend(_frontend, _state)
{
const AP_SerialManager &serial_manager = AP::serialmanager();
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0));
}
}
// detect if a TeraRanger Tower proximity sensor is connected by looking for a configured serial port
bool AP_Proximity_NanaRadar_MR72::detect()
{
AP_HAL::UARTDriver *uart = nullptr;
uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
return uart != nullptr;
}
// update the state of the sensor
void AP_Proximity_NanaRadar_MR72::update(void)
{
if (uart == nullptr) {
return;
}
// process incoming messages
read_sensor_data();
// check for timeout and set health status
if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_TRTOWER_TIMEOUT_MS)) {
set_status(AP_Proximity::Status::NoData);
} else {
set_status(AP_Proximity::Status::Good);
}
}
// get maximum and minimum distances (in meters) of primary sensor
float AP_Proximity_NanaRadar_MR72::distance_max() const
{
return 40.0f;
}
float AP_Proximity_NanaRadar_MR72::distance_min() const
{
return 0.20f;
}
// check for replies from sensor, returns true if at least one message was processed
bool AP_Proximity_NanaRadar_MR72::read_sensor_data()
{
if (uart == nullptr) {
return false;
}
uint16_t message_count = 0;
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
// uint8_t c = uart->read();
// gcs().send_text(MAV_SEVERITY_INFO, "getc 0x%02x",c);
if (uart->read() == Head1)
{ //判断数据包帧头0x54
buffer[0] = Head1;
// gcs().send_text(MAV_SEVERITY_INFO, "getc 0x%02x",buffer[0]);
if (uart->read() == Head2)
{ //判断数据包帧头0X48
buffer[1] = Head2;
// gcs().send_text(MAV_SEVERITY_INFO, "getc 0x%02x",buffer[1]);
for (int i = 2; i < 19; i++)
{ //存储数据到数组
buffer[i] = uart->read();
// gcs().send_text(MAV_SEVERITY_INFO, "%02d: 0x%02x",i,buffer[i]);
}
CheckSum = crc_crc8(buffer,18);
// gcs().send_text(MAV_SEVERITY_INFO, "crc 0x%02x, rec:0x%02x",CheckSum,buffer[18]);
if (buffer[18] == CheckSum)
{ //按照协议对收到的数据进行校验
update_sector_data(0, UINT16_VALUE(buffer[2], buffer[3])); // d1
update_sector_data(45, UINT16_VALUE(buffer[4], buffer[5])); // d8
update_sector_data(90, UINT16_VALUE(buffer[6], buffer[7])); // d7
update_sector_data(135, UINT16_VALUE(buffer[8], buffer[9])); // d6
update_sector_data(180, UINT16_VALUE(buffer[10], buffer[11])); // d5
update_sector_data(225, UINT16_VALUE(buffer[12], buffer[13])); // d4
update_sector_data(270, UINT16_VALUE(buffer[14], buffer[15])); // d3
update_sector_data(315, UINT16_VALUE(buffer[16], buffer[17])); // d2
message_count++;
}
}
}
}
return (message_count > 0);
}
// process reply
void AP_Proximity_NanaRadar_MR72::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
{
uint8_t sector;
if (convert_angle_to_sector(angle_deg, sector)) {
_angle[sector] = angle_deg;
_distance[sector] = ((float) distance_cm) / 100;
_distance_valid[sector] = distance_cm != 0xffff;
_last_distance_received_ms = AP_HAL::millis();
// update boundary used for avoidance
update_boundary_for_sector(sector, true);
}
}

42
libraries/AP_Proximity/AP_Proximity_NanaRadar_MR72.h
Unescape View File

@ -0,0 +1,42 @@ @@ -0,0 +1,42 @@
#pragma once
#include "AP_Proximity.h"
#include "AP_Proximity_Backend.h"
#define PROXIMITY_TRTOWER_TIMEOUT_MS 300 // requests timeout after 0.3 seconds
class AP_Proximity_NanaRadar_MR72 : public AP_Proximity_Backend
{
public:
// constructor
AP_Proximity_NanaRadar_MR72(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state);
// static detection function
static bool detect();
// update state
void update(void) override;
// get maximum and minimum distances (in meters) of sensor
float distance_max() const override;
float distance_min() const override;
private:
// check and process replies from sensor
bool read_sensor_data();
void update_sector_data(int16_t angle_deg, uint16_t distance_cm);
// reply related variables
AP_HAL::UARTDriver *uart = nullptr;
uint8_t buffer[20]; // buffer where to store data from serial
uint8_t buffer_count;
uint8_t CheckSum;//校验数值存放
const int Head1=0x54; //数据包帧头
const int Head2=0X48;
// request related variables
uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor
};

161
libraries/AP_RangeFinder/AP_RangeFinder_NanoRadar_MR72.cpp
Unescape View File

@ -0,0 +1,161 @@ @@ -0,0 +1,161 @@
/*
* Copyright (C) 2016 Intel Corporation. All rights reserved.
*
* This file 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 file 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 <ctype.h>
#include "AP_RangeFinder_NanoRadar_MR72.h"
#define MAXSONAR_SERIAL_LV_BAUD_RATE 9600
extern const AP_HAL::HAL &hal;
static const uint8_t crc8_table[] = {
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31,
0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9,
0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1,
0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,
0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,
0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,
0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f,
0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7,
0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
0xfa, 0xfd, 0xf4, 0xf3};
/*
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_NanoRadar_MR72::AP_RangeFinder_NanoRadar_MR72(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
uint8_t serial_instance) : AP_RangeFinder_Backend(_state, _params)
{
const AP_SerialManager &serial_manager = AP::serialmanager();
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 MaxSonar rangefinder is connected. We'll detect by
trying to take a reading on Serial. If we get a result the sensor is
there.
*/
bool AP_RangeFinder_NanoRadar_MR72::detect(uint8_t serial_instance)
{
return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr;
}
// read - return last value measured by sensor
bool AP_RangeFinder_NanoRadar_MR72::get_reading(uint16_t &reading_cm)
{
if (uart == nullptr)
{
return false;
}
uint8_t frameOK = 0;
uint16_t Range = 0; //雷达实测距离
int16_t nbytes = uart->available();
while (nbytes-- > 0)
{
// uint8_t c = uart->read();
// gcs().send_text(MAV_SEVERITY_INFO, "getc 0x%02x",c);
if (uart->read() == Head1)
{ //判断数据包帧头0x54
string[0] = Head1;
// gcs().send_text(MAV_SEVERITY_INFO, "getc 0x%02x",string[0]);
if (uart->read() == Head2)
{ //判断数据包帧头0X48
string[1] = Head2;
// gcs().send_text(MAV_SEVERITY_INFO, "getc 0x%02x",string[1]);
for (int i = 2; i < 19; i++)
{ //存储数据到数组
string[i] = uart->read();
// gcs().send_text(MAV_SEVERITY_INFO, "%02d: 0x%02x",i,string[i]);
}
CheckSum = crc_crc8(string,18);
// gcs().send_text(MAV_SEVERITY_INFO, "crc 0x%02x, rec:0x%02x",CheckSum,string[18]);
if (string[18] == CheckSum)
{ //按照协议对收到的数据进行校验
frameOK = 1;
Range = (string[2] * 256 + string[3]); //cm
// gcs().send_text(MAV_SEVERITY_INFO, "range: %dcm",Range);
// testdata = (string[8]*256+string[9]);
}
}
}
}
if (frameOK == 0)
{
return false;
}
if (Range != 0)
{
reading_cm = Range;
}
return true;
}
/*
update the state of the sensor
*/
void AP_RangeFinder_NanoRadar_MR72::update(void)
{
if (get_reading(state.distance_cm))
{
// update range_valid state based on distance measured
state.last_reading_ms = AP_HAL::millis();
update_status();
}
else if (AP_HAL::millis() - state.last_reading_ms > 500)
{
set_status(RangeFinder::RangeFinder_NoData);
}
}
/*
crc8 from trone driver by Luis Rodrigues
*/
uint8_t AP_RangeFinder_NanoRadar_MR72::crc_crc8(const uint8_t *p, uint8_t len)
{
uint16_t i;
uint16_t crc = 0x0;
while (len--)
{
i = (crc ^ *p++) & 0xFF;
crc = (crc8_table[i] ^ (crc << 8)) & 0xFF;
}
return crc & 0xFF;
}

39
libraries/AP_RangeFinder/AP_RangeFinder_NanoRadar_MR72.h
Unescape View File

@ -0,0 +1,39 @@ @@ -0,0 +1,39 @@
#pragma once
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
class AP_RangeFinder_NanoRadar_MR72 : public AP_RangeFinder_Backend
{
public:
// constructor
AP_RangeFinder_NanoRadar_MR72(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
uint8_t serial_instance);
// static detection function
static bool detect(uint8_t serial_instance);
// update state
void update(void) override;
protected:
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_ULTRASOUND;
}
private:
// get a reading
bool get_reading(uint16_t &reading_cm);
uint8_t crc_crc8(const uint8_t *p, uint8_t len);
AP_HAL::UARTDriver *uart = nullptr;
char linebuf[10];
uint8_t linebuf_len = 0;
const int Head1=0x54; //数据包帧头
const int Head2=0X48;
uint8_t string[19],CheckSum;//校验数值存放
};

110
libraries/AP_RangeFinder/AP_RangeFinder_insighticaSerial.cpp
Unescape View File

@ -0,0 +1,110 @@ @@ -0,0 +1,110 @@
/*
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_RangeFinder_insighticaSerial.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <ctype.h>
#include "stdio.h"
extern const AP_HAL::HAL& hal;
/*
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_insighticaSerial::AP_RangeFinder_insighticaSerial(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state, _params)
{
const AP_SerialManager &serial_manager = AP::serialmanager();
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 insightica rangefinder is connected. We'll detect by
trying to take a reading on Serial. If we get a result the sensor is
there.
*/
bool AP_RangeFinder_insighticaSerial::detect(uint8_t serial_instance)
{
return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr;
}
const int StartSequence=0xAA; //数据包帧头
const int EndSequence=0X55;
// read - return last value measured by sensor
// bool AP_RangeFinder_insighticaSerial::get_reading(uint16_t &reading_cm,uint16_t &testdata)
bool AP_RangeFinder_insighticaSerial::get_reading(uint16_t &reading_cm)
{
if (uart == nullptr) {
return false;
}
// read any available lines from the lidar
uint8_t frameOK = 0;
uint16_t Range = 0; //雷达实测距离
int16_t nbytes = uart->available();
while ( nbytes-- > 0 ) {
if( uart->read() == StartSequence ){ //判断数据包帧头0xAA
string[0] = StartSequence;
if( uart->read() == StartSequence ){ //判断数据包帧头0xAA
string[1] = StartSequence;
for( int i = 2;i < 14;i++ ){ //存储数据到数组
string[i] = uart->read();
}
CheckSum = string[4]+string[5]+string[6]+string[7]+string[8]+string[9]+string[10];
if( string[11] == (CheckSum&0xFF)&&string[12] == EndSequence && string[13] == EndSequence ){ //按照协议对收到的数据进行校验
frameOK = 1;
Range = (string[6]*256+string[7]); //cm
// testdata = (string[8]*256+string[9]);
}
}
}
}
if (frameOK == 0)
{
return false;
}
if(Range != 0)
{
reading_cm = Range;
}
return true;
}
/*
update the state of the sensor
****** this is used by the up code of sensors.cpp
*/
void AP_RangeFinder_insighticaSerial::update(void)
{
if (get_reading(state.distance_cm)) {
// update range_valid state based on distance measured
state.last_reading_ms = AP_HAL::millis();
update_status();
} else if (AP_HAL::millis() - state.last_reading_ms > 500) {
set_status(RangeFinder::RangeFinder_NoData);
}
}

36
libraries/AP_RangeFinder/AP_RangeFinder_insighticaSerial.h
Unescape View File

@ -0,0 +1,36 @@ @@ -0,0 +1,36 @@
#pragma once
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
class AP_RangeFinder_insighticaSerial : public AP_RangeFinder_Backend
{
public:
// constructor
AP_RangeFinder_insighticaSerial(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
uint8_t serial_instance);
// static detection function
static bool detect(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_LASER;
}
private:
// get a reading
// bool get_reading(uint16_t &reading_cm,uint16_t &testdata);
bool get_reading(uint16_t &reading_cm);
AP_HAL::UARTDriver *uart = nullptr;
uint32_t last_reading_ms = 0;
uint8_t string[14],CheckSum;//校验数值存放
};
Write Preview
Loading…
Cancel
Save