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

AP_RangeFinder: remove primary, add orientation

This commit is contained in:
Randy Mackay 2017-02-09 19:26:57 +09:00
parent 49f4afc2a1
commit e7dd21a0bb
2 changed files with 162 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

154
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -152,6 +152,13 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @User: Advanced
AP_GROUPINFO("_POS", 49, RangeFinder, _pos_offset[0], 0.0f),
// @Param: _ORIENT
// @DisplayName: Rangefinder orientation
// @Description: Orientation of rangefinder
// @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("_ORIENT", 53, RangeFinder, _orientation[0], ROTATION_PITCH_270),
#if RANGEFINDER_MAX_INSTANCES > 1
// @Param: 2_TYPE
// @DisplayName: Second Rangefinder type
@ -264,6 +271,12 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @User: Advanced
AP_GROUPINFO("2_POS", 50, RangeFinder, _pos_offset[1], 0.0f),
// @Param: 2_ORIENT
// @DisplayName: Rangefinder 2 orientation
// @Description: Orientation of 2nd rangefinder
// @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, _orientation[1], ROTATION_PITCH_270),
#endif
#if RANGEFINDER_MAX_INSTANCES > 2
@ -379,6 +392,12 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @User: Advanced
AP_GROUPINFO("3_POS", 51, RangeFinder, _pos_offset[2], 0.0f),
// @Param: 3_ORIENT
// @DisplayName: Rangefinder 3 orientation
// @Description: Orientation of 3rd rangefinder
// @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, _orientation[2], ROTATION_PITCH_270),
#endif
#if RANGEFINDER_MAX_INSTANCES > 3
@ -493,19 +512,30 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Units: m
// @User: Advanced
AP_GROUPINFO("4_POS", 52, RangeFinder, _pos_offset[3], 0.0f),
// @Param: 4_ORIENT
// @DisplayName: Rangefinder 4 orientation
// @Description: Orientation of 4th range finder
// @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, _orientation[3], ROTATION_PITCH_270),
#endif
AP_GROUPEND
};
RangeFinder::RangeFinder(AP_SerialManager &_serial_manager) :
primary_instance(0),
RangeFinder::RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orientation_default) :
num_instances(0),
estimated_terrain_height(0),
serial_manager(_serial_manager)
{
AP_Param::setup_object_defaults(this, var_info);
// set orientation defaults
for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
_orientation[i].set_default(orientation_default);
}
// init state and drivers
memset(state,0,sizeof(state));
memset(drivers,0,sizeof(drivers));
@ -558,13 +588,6 @@ void RangeFinder::update(void)
update_pre_arm_check(i);
}
}
// work out primary instance - first sensor returning good data
for (int8_t i=num_instances-1; i>=0; i--) {
if (drivers[i] != nullptr && (state[i].status == RangeFinder_Good)) {
primary_instance = i;
}
}
}
bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend)
@ -688,13 +711,87 @@ RangeFinder::RangeFinder_Status RangeFinder::status(uint8_t instance) const
return state[instance].status;
}
void RangeFinder::handle_msg(mavlink_message_t *msg) {
uint8_t i;
for (i=0; i<num_instances; i++) {
if ((drivers[i] != nullptr) && (_type[i] != RangeFinder_TYPE_NONE)) {
RangeFinder::RangeFinder_Status RangeFinder::status_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return status(i);
}
return RangeFinder_NotConnected;
}
void RangeFinder::handle_msg(mavlink_message_t *msg)
{
uint8_t i;
for (i=0; i<num_instances; i++) {
if ((drivers[i] != nullptr) && (_type[i] != RangeFinder_TYPE_NONE)) {
drivers[i]->handle_msg(msg);
}
}
}
}
}
// return true if we have a range finder with the specified orientation
bool RangeFinder::has_orientation(enum Rotation orientation) const
{
uint8_t i;
return find_instance(orientation, i);
}
// find first range finder instance with the specified orientation
bool RangeFinder::find_instance(enum Rotation orientation, uint8_t &instance) const
{
for (uint8_t i=0; i<num_instances; i++) {
if (_orientation[i] == orientation) {
instance = i;
return true;
}
}
return false;
}
uint16_t RangeFinder::distance_cm_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return distance_cm(i);
}
return 0;
}
uint16_t RangeFinder::voltage_mv_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return voltage_mv(i);
}
return 0;
}
int16_t RangeFinder::max_distance_cm_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return max_distance_cm(i);
}
return 0;
}
int16_t RangeFinder::min_distance_cm_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return min_distance_cm(i);
}
return 0;
}
int16_t RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return ground_clearance_cm(i);
}
return 0;
}
// true if sensor is returning data
@ -707,6 +804,24 @@ bool RangeFinder::has_data(uint8_t instance) const
return ((state[instance].status != RangeFinder_NotConnected) && (state[instance].status != RangeFinder_NoData));
}
bool RangeFinder::has_data_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return has_data(i);
}
return false;
}
uint8_t RangeFinder::range_valid_count_orient(enum Rotation orientation) const
{
uint8_t i;
if (find_instance(orientation, i)) {
return range_valid_count(i);
}
return 0;
}
/*
returns true if pre-arm checks have passed for all range finders
these checks involve the user lifting or rotating the vehicle so that sensor readings between
@ -748,3 +863,12 @@ void RangeFinder::update_pre_arm_check(uint8_t instance)
state[instance].pre_arm_check = true;
}
}
const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) const
{
uint8_t i=0;
if (find_instance(orientation, i)) {
return get_pos_offset(i);
}
return pos_offset_zero;
}

55
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -33,7 +33,7 @@ class RangeFinder
public:
friend class AP_RangeFinder_Backend;
RangeFinder(AP_SerialManager &_serial_manager);
RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orientation_default);
// RangeFinder driver types
enum RangeFinder_Type {
@ -96,6 +96,7 @@ public:
AP_Int8 _address[RANGEFINDER_MAX_INSTANCES];
AP_Int16 _powersave_range;
AP_Vector3f _pos_offset[RANGEFINDER_MAX_INSTANCES]; // position offset in body frame
AP_Int8 _orientation[RANGEFINDER_MAX_INSTANCES];
static const struct AP_Param::GroupInfo var_info[];
@ -116,63 +117,55 @@ public:
#define _RangeFinder_STATE(instance) state[instance]
uint8_t get_primary(void) const {
return primary_instance;
// return true if we have a range finder with the specified orientation
bool has_orientation(enum Rotation orientation) const;
// find first range finder instance with the specified orientation
bool find_instance(enum Rotation orientation, uint8_t &instance) const;
// get orientation of a given range finder
enum Rotation get_orientation(uint8_t instance) const {
return (instance<num_instances? (enum Rotation)_orientation[instance].get() : ROTATION_NONE);
}
uint16_t distance_cm(uint8_t instance) const {
return (instance<num_instances? _RangeFinder_STATE(instance).distance_cm : 0);
}
uint16_t distance_cm() const {
return distance_cm(primary_instance);
}
uint16_t distance_cm_orient(enum Rotation orientation) const;
uint16_t voltage_mv(uint8_t instance) const {
return _RangeFinder_STATE(instance).voltage_mv;
}
uint16_t voltage_mv() const {
return voltage_mv(primary_instance);
}
uint16_t voltage_mv_orient(enum Rotation orientation) const;
int16_t max_distance_cm(uint8_t instance) const {
return _max_distance_cm[instance];
}
int16_t max_distance_cm() const {
return max_distance_cm(primary_instance);
}
int16_t max_distance_cm_orient(enum Rotation orientation) const;
int16_t min_distance_cm(uint8_t instance) const {
return _min_distance_cm[instance];
}
int16_t min_distance_cm() const {
return min_distance_cm(primary_instance);
}
int16_t min_distance_cm_orient(enum Rotation orientation) const;
int16_t ground_clearance_cm(uint8_t instance) const {
return _ground_clearance_cm[instance];
}
int16_t ground_clearance_cm() const {
return _ground_clearance_cm[primary_instance];
}
int16_t ground_clearance_cm_orient(enum Rotation orientation) const;
// query status
RangeFinder_Status status(uint8_t instance) const;
RangeFinder_Status status(void) const {
return status(primary_instance);
}
RangeFinder_Status status_orient(enum Rotation orientation) const;
// true if sensor is returning data
bool has_data(uint8_t instance) const;
bool has_data() const {
return has_data(primary_instance);
}
bool has_data_orient(enum Rotation orientation) const;
// returns count of consecutive good readings
uint8_t range_valid_count() const {
return range_valid_count(primary_instance);
}
uint8_t range_valid_count(uint8_t instance) const {
return _RangeFinder_STATE(instance).range_valid_count;
}
uint8_t range_valid_count_orient(enum Rotation orientation) const;
/*
set an externally estimated terrain height. Used to enable power
@ -193,17 +186,15 @@ public:
const Vector3f &get_pos_offset(uint8_t instance) const {
return _pos_offset[instance];
}
const Vector3f &get_pos_offset(void) const {
return _pos_offset[primary_instance];
}
const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
private:
RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
uint8_t primary_instance:3;
uint8_t num_instances:3;
float estimated_terrain_height;
AP_SerialManager &serial_manager;
Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests
void detect_instance(uint8_t instance);
void update_instance(uint8_t instance);