mirror of https://github.com/ArduPilot/ardupilot
103 lines
3.1 KiB
C++
103 lines
3.1 KiB
C++
/*
|
|
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/>.
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "AP_RangeFinder_config.h"
|
|
|
|
#if AP_RANGEFINDER_RDS02UF_ENABLED
|
|
|
|
#include "AP_RangeFinder.h"
|
|
#include "AP_RangeFinder_Backend_Serial.h"
|
|
|
|
#define RDS02_BUFFER_SIZE 50
|
|
#define RDS02UF_DIST_MAX_CM 2000
|
|
#define RDS02UF_DIST_MIN_CM 150
|
|
#define RDS02UF_DATA_LEN 10
|
|
|
|
class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial
|
|
{
|
|
|
|
public:
|
|
|
|
static AP_RangeFinder_Backend_Serial *create(
|
|
RangeFinder::RangeFinder_State &_state,
|
|
AP_RangeFinder_Params &_params) {
|
|
return new AP_RangeFinder_RDS02UF(_state, _params);
|
|
}
|
|
|
|
protected:
|
|
|
|
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
|
|
return MAV_DISTANCE_SENSOR_RADAR;
|
|
}
|
|
|
|
private:
|
|
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
|
|
|
|
// find a RDS02UF message in the buffer, starting at
|
|
// initial_offset. If found, that message (or partial message) will
|
|
// be moved to the start of the buffer.
|
|
void move_header_in_buffer(uint8_t initial_offset);
|
|
|
|
// get a distance reading
|
|
bool get_reading(float &reading_m) override;
|
|
uint16_t read_timeout_ms() const override { return 500; }
|
|
|
|
// make sure readings go out-of-range when necessary
|
|
int16_t max_distance_cm()const override { return MIN(params.max_distance_cm, RDS02UF_DIST_MAX_CM); }
|
|
int16_t min_distance_cm() const override { return MAX(params.min_distance_cm, RDS02UF_DIST_MIN_CM); }
|
|
|
|
// Data Format for Benewake Rds02UF
|
|
// ===============================
|
|
// 21 bytes total per message:
|
|
// 1) 0x55
|
|
// 2) 0x55
|
|
// 3) address
|
|
// 4) error_code
|
|
// 5) FC_CODE_L (low 8bit)
|
|
// 6) FC_CODE_H (high 8bit)
|
|
// 7) LENGTH_L (low 8bit)
|
|
// 8) LENGTH_H (high 8bit)
|
|
// 9) REAL_DATA (10Byte)
|
|
// 10) CRC8
|
|
// 11) END_1 0xAA
|
|
// 12) END_2 0xAA
|
|
struct PACKED RDS02UFPacket {
|
|
uint8_t headermagic1;
|
|
uint8_t headermagic2;
|
|
uint8_t address;
|
|
uint8_t error_code;
|
|
uint8_t fc_low;
|
|
uint8_t fc_high;
|
|
uint8_t length_l;
|
|
uint8_t length_h;
|
|
uint8_t data[RDS02UF_DATA_LEN];
|
|
uint8_t checksum;
|
|
uint8_t footermagic1;
|
|
uint8_t footermagic2;
|
|
};
|
|
|
|
union RDS02UF_Union {
|
|
uint8_t parse_buffer[21];
|
|
struct RDS02UFPacket packet;
|
|
};
|
|
RDS02UF_Union u;
|
|
|
|
// number of bytes currently in the buffer
|
|
uint8_t body_length;
|
|
};
|
|
#endif // AP_RANGEFINDER_RDS02UF_ENABLED
|