Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Rangefinder: Add WASP 200 LRF backend

Browse Source
This commit is contained in:
Michael du Breuil 2018-05-16 15:31:22 -07:00 committed by Francisco Ferreira
parent f2ac48a33e
commit b678302aed
4 changed files with 340 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

257
libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp Normal file
View File

@ -0,0 +1,257 @@
/*
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_Wasp.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <ctype.h>
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_RangeFinder_Wasp::var_info[] = {
// @Param: WSP_MAVG
// @DisplayName: Moving Average Range
// @Description: Sets the number of historic range results to use for calculating the current range result. When MAVG is greater than 1, the current range result will be the current measured value averaged with the N-1 previous results
// @Range 0-255
// @User: Advanced
AP_GROUPINFO("WSP_MAVG", 1, AP_RangeFinder_Wasp, mavg, 4),
// @Param: WSP_MEDF
// @DisplayName: Moving Median Filter
// @Description: Sets the window size for the real-time median filter. When MEDF is greater than 0 the median filter is active
// @Range 0-255
// @User: Advanced
AP_GROUPINFO("WSP_MEDF", 2, AP_RangeFinder_Wasp, medf, 4),
// @Param: WSP_FRQ
// @DisplayName: Frequency
// @Description: Sets the repetition frequency of the ranging operation in Hertz. Upon entering the desired frequency the system will calculate the nearest frequency that it can handle according to the resolution of internal timers.
// @Range 0-10000
// @User: Advanced
AP_GROUPINFO("WSP_FRQ", 3, AP_RangeFinder_Wasp, frq, 100),
// @Param: WSP_AVG
// @DisplayName: Multi-pulse averages
// @Description: Sets the number of pulses to be used in multi-pulse averaging mode. In this mode, a sequence of rapid fire ranges are taken and then averaged to improve the accuracy of the measurement
// @Range 0-255
// @User: Advanced
AP_GROUPINFO("WSP_AVG", 4, AP_RangeFinder_Wasp, avg, 4),
// @Param: WSP_THR
// @DisplayName: Sensitivity threshold
// @Description: Sets the system sensitivity. Larger values of THR represent higher sensitivity. The system may limit the maximum value of THR to prevent excessive false alarm rates based on settings made at the factory. Set to -1 for automatic threshold adjustments
// @Range -1-255
// @User: Advanced
AP_GROUPINFO("WSP_THR", 5, AP_RangeFinder_Wasp, thr, -1),
AP_GROUPEND
};
AP_RangeFinder_Wasp::AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager,
uint8_t serial_instance) :
AP_RangeFinder_Backend(_state) {
AP_Param::setup_object_defaults(this, var_info);
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
if (uart != nullptr) {
uart->begin(115200);
// register Wasp specific parameters
state.var_info = var_info;
configuration_state = WASP_CFG_PROTOCOL;
}
}
// detection is considered as locating a serial port
bool AP_RangeFinder_Wasp::detect(AP_SerialManager &serial_manager, uint8_t serial_instance) {
return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr;
}
// read - return last value measured by sensor
bool AP_RangeFinder_Wasp::get_reading(uint16_t &reading_cm) {
if (uart == nullptr) {
return false;
}
// read any available lines from the lidar
float sum = 0;
uint16_t count = 0;
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
char c = uart->read();
if (c == '\n') {
linebuf[linebuf_len] = 0;
linebuf_len = 0;
last_reading_ms = AP_HAL::millis();
if (isalpha(linebuf[0])) {
parse_response();
} else {
float read_value = (float)atof(linebuf);
if (read_value > 0) {
sum += read_value;
count++;
}
}
} else if (isalnum(c) || c == '.' || c == '-') {
linebuf[linebuf_len++] = c;
}
// discard excessively long buffers
if (linebuf_len == sizeof(linebuf)) {
linebuf_len = 0;
}
}
if (configuration_state == WASP_CFG_RATE && uart->tx_pending() == 0) {
configuration_state = WASP_CFG_ENCODING;
}
if (count == 0) {
return false;
}
reading_cm = 100 * sum / count;
set_status(RangeFinder::RangeFinder_Good);
return true;
}
#define COMMAND_BUFFER_LEN 15
void AP_RangeFinder_Wasp::update(void) {
if (!get_reading(state.distance_cm)) {
set_status(RangeFinder::RangeFinder_NoData);
}
if (AP_HAL::millis() - last_reading_ms > 500) {
// attempt to reconfigure on the assumption this was a bad baud setting
configuration_state = WASP_CFG_RATE;
}
char command[COMMAND_BUFFER_LEN] = {};
switch (configuration_state) {
case WASP_CFG_RATE:
hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">BAUD %s\n", baud > 0 ? "HIGH" : "LOW");
break;
case WASP_CFG_ENCODING:
uart->end();
uart->begin(baud > 0 ? 912600 : 115200);
hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">LBE LITTLE\n");
break;
case WASP_CFG_PROTOCOL:
hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">FMT ASCII\n");
break;
case WASP_CFG_FRQ:
hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">FRQ %d\n", constrain_int16(frq, 20, baud > 0 ? 10000 : 1440));
break;
case WASP_CFG_GO:
hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">GO\n");
break;
case WASP_CFG_AUT:
hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">AUT %d\n", thr >= 0 ? 0 : 1);
break;
case WASP_CFG_THR:
if (thr >= 0) {
hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">THR %d\n", constrain_int16(thr, 0,255));
} else {
configuration_state = WASP_CFG_MAVG;
}
break;
case WASP_CFG_MAVG:
hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">MAVG %d\n", constrain_int16(mavg, 0, 255));
break;
case WASP_CFG_MEDF:
hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">MEDF %d\n", constrain_int16(medf, 0, 255));
break;
case WASP_CFG_AVG:
hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">AVG %d\n", constrain_int16(avg, 0, 255));
break;
case WASP_CFG_AUV:
hal.util->snprintf(command, COMMAND_BUFFER_LEN, ">AUV 1\n");
break;
case WASP_CFG_DONE:
return;
}
if (command[0] != 0) {
uart->write((uint8_t *)command, strlen(command));
}
}
void AP_RangeFinder_Wasp::parse_response(void) {
switch (configuration_state) {
case WASP_CFG_RATE:
configuration_state = WASP_CFG_ENCODING;
break;
case WASP_CFG_ENCODING:
if (strncmp(linebuf, "LBE", 3) == 0) {
configuration_state = WASP_CFG_PROTOCOL;
}
break;
case WASP_CFG_PROTOCOL:
if (strncmp(linebuf, "FMT", 3) == 0) {
configuration_state = WASP_CFG_FRQ;
}
break;
case WASP_CFG_FRQ:
if (strncmp(linebuf, "FRQ", 3) == 0) {
configuration_state = WASP_CFG_GO;
}
break;
case WASP_CFG_GO:
if (strncmp(linebuf, "GO", 2) == 0) {
configuration_state = WASP_CFG_AUT;
}
break;
case WASP_CFG_AUT:
if (strncmp(linebuf, "AUT", 3) == 0) {
configuration_state = WASP_CFG_THR;
}
break;
case WASP_CFG_THR:
if (strncmp(linebuf, "THR", 3) == 0) {
configuration_state = WASP_CFG_MAVG;
}
break;
case WASP_CFG_MAVG:
if (strncmp(linebuf, "MAVG", 4) == 0) {
configuration_state = WASP_CFG_MEDF;
}
break;
case WASP_CFG_MEDF:
if (strncmp(linebuf, "MEDF", 4) == 0) {
configuration_state = WASP_CFG_AVG;
}
break;
case WASP_CFG_AVG:
if (strncmp(linebuf, "AVG", 3) == 0) {
configuration_state = WASP_CFG_AUV;
}
break;
case WASP_CFG_AUV:
if (strncmp(linebuf, "AUV", 3) == 0) {
configuration_state = WASP_CFG_DONE;
}
break;
case WASP_CFG_DONE:
return;
}
}

61
libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h Normal file
View File

@ -0,0 +1,61 @@
#pragma once
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
// WASP 200 LRF
// http://www.attolloengineering.com/wasp-200-lrf.html
class AP_RangeFinder_Wasp : public AP_RangeFinder_Backend {
public:
AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager,
uint8_t serial_instance);
static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance);
void update(void) override;
static const struct AP_Param::GroupInfo var_info[];
protected:
virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_LASER;
}
private:
enum wasp_configuration_stage {
WASP_CFG_RATE, // set the baudrate
WASP_CFG_ENCODING, // set the encoding to LBE
WASP_CFG_PROTOCOL, // set the protocol type used
WASP_CFG_FRQ, // set the update frequency
WASP_CFG_GO, // start/resume readings
WASP_CFG_AUT, // set the auto sensitivity threshold
WASP_CFG_THR, // set the sensitivity threshold
WASP_CFG_MAVG, // set the moving average filter
WASP_CFG_MEDF, // set the median filter windows size
WASP_CFG_AVG, // set the multi-pulse averages
WASP_CFG_AUV, // enforce auto voltage
WASP_CFG_DONE // done configuring
};
wasp_configuration_stage configuration_state;
bool get_reading(uint16_t &reading_cm);
void parse_response(void);
AP_HAL::UARTDriver *uart;
uint32_t last_reading_ms;
char linebuf[10];
uint8_t linebuf_len;
AP_Int16 mavg;
AP_Int16 medf;
AP_Int16 frq;
AP_Int16 avg;
AP_Int16 thr;
AP_Int8 baud;
};

24
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -33,6 +33,7 @@
#include "AP_RangeFinder_TeraRangerI2C.h"
#include "AP_RangeFinder_VL53L0X.h"
#include "AP_RangeFinder_NMEA.h"
#include "AP_RangeFinder_Wasp.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
extern const AP_HAL::HAL &hal;
@ -42,7 +43,7 @@ const AP_Param::GroupInfo RangeFinder::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
// @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-200
// @User: Standard
AP_GROUPINFO("_TYPE", 0, RangeFinder, state[0].type, 0),
@ -165,11 +166,13 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_ORIENT", 53, RangeFinder, state[0].orientation, ROTATION_PITCH_270),
AP_SUBGROUPVARPTR(drivers[0], "_", 57, RangeFinder, backend_var_info[0]),
#if RANGEFINDER_MAX_INSTANCES > 1
// @Param: 2_TYPE
// @DisplayName: Second 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
// @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-200
// @User: Advanced
AP_GROUPINFO("2_TYPE", 12, RangeFinder, state[1].type, 0),
@ -283,6 +286,8 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
AP_GROUPINFO("2_ORIENT", 54, RangeFinder, state[1].orientation, ROTATION_PITCH_270),
AP_SUBGROUPVARPTR(drivers[1], "2_", 58, RangeFinder, backend_var_info[1]),
#endif
#if RANGEFINDER_MAX_INSTANCES > 2
@ -290,7 +295,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: 3_TYPE
// @DisplayName: Third 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
// @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-200
// @User: Advanced
AP_GROUPINFO("3_TYPE", 25, RangeFinder, state[2].type, 0),
@ -404,6 +409,8 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
AP_GROUPINFO("3_ORIENT", 55, RangeFinder, state[2].orientation, ROTATION_PITCH_270),
AP_SUBGROUPVARPTR(drivers[2], "3_", 59, RangeFinder, backend_var_info[2]),
#endif
#if RANGEFINDER_MAX_INSTANCES > 3
@ -411,7 +418,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: 4_TYPE
// @DisplayName: Fourth 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
// @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-200
// @User: Advanced
AP_GROUPINFO("4_TYPE", 37, RangeFinder, state[3].type, 0),
@ -525,8 +532,10 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced
AP_GROUPINFO("4_ORIENT", 56, RangeFinder, state[3].orientation, ROTATION_PITCH_270),
AP_SUBGROUPVARPTR(drivers[3], "4_", 60, RangeFinder, backend_var_info[3]),
#endif
AP_GROUPEND
};
@ -720,6 +729,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
case RangeFinder_TYPE_NMEA:
if (AP_RangeFinder_NMEA::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_NMEA(state[instance], serial_manager, serial_instance++);
break;
}
case RangeFinder_TYPE_WASP:
if (AP_RangeFinder_Wasp::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_Wasp(state[instance], serial_manager, serial_instance++);
}
break;
default:

4
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -58,7 +58,8 @@ public:
RangeFinder_TYPE_TRI2C = 14,
RangeFinder_TYPE_PLI2CV3= 15,
RangeFinder_TYPE_VL53L0X = 16,
RangeFinder_TYPE_NMEA = 17
RangeFinder_TYPE_NMEA = 17,
RangeFinder_TYPE_WASP = 18,
};
enum RangeFinder_Function {
@ -100,6 +101,7 @@ public:
AP_Int8 address;
AP_Vector3f pos_offset; // position offset in body frame
AP_Int8 orientation;
const struct AP_Param::GroupInfo *backend_var_info;
};
AP_Int16 _powersave_range;