aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Proximity: fix consumption of distance-sensor messages

Browse Source
This commit is contained in:
Randy Mackay 2017-01-10 12:09:18 +09:00
parent 742cdf6b13
commit a0a8472a44
5 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -242,6 +242,16 @@ AP_Proximity::Proximity_Status AP_Proximity::get_status() const
return get_status(primary_instance);
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Proximity::handle_msg(mavlink_message_t *msg)
{
for (uint8_t i=0; i<num_instances; i++) {
if ((drivers[i] != nullptr) && (_type[i] != Proximity_Type_None)) {
drivers[i]->handle_msg(msg);
}
}
}
// detect if an instance of a proximity sensor is connected.
void AP_Proximity::detect_instance(uint8_t instance)
{
@ -340,7 +350,7 @@ bool AP_Proximity::get_distances(Proximity_Distance_Array &prx_dist_array) const
if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) {
return 0.0f;
}
// get maximum distance from backend
// get distances from backend
return drivers[primary_instance]->get_distances(prx_dist_array);
}

3
libraries/AP_Proximity/AP_Proximity.h
View File

@ -98,6 +98,9 @@ public:
float distance_max() const;
float distance_min() const;
// handle mavlink DISTANCE_SENSOR messages
void handle_msg(mavlink_message_t *msg);
// The Proximity_State structure is filled in by the backend driver
struct Proximity_State {
uint8_t instance; // the instance number of this proximity sensor

3
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -38,6 +38,9 @@ public:
virtual float distance_max() const = 0;
virtual float distance_min() const = 0;
// handle mavlink DISTANCE_SENSOR messages
virtual void handle_msg(mavlink_message_t *msg) {}
// get distance in meters in a particular direction in degrees (0 is forward, clockwise)
// returns true on successful read and places distance in distance
bool get_horizontal_distance(float angle_deg, float &distance) const;

1
libraries/AP_Proximity/AP_Proximity_MAV.cpp
View File

@ -58,5 +58,6 @@ void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg)
_distance_min = packet.min_distance / 100.0f;
_distance_max = packet.max_distance / 100.0f;
_last_update_ms = AP_HAL::millis();
update_boundary_for_sector(sector);
}
}

2
libraries/AP_Proximity/AP_Proximity_MAV.h
View File

@ -20,7 +20,7 @@ public:
float distance_min() const { return _distance_min; };
// handle mavlink DISTANCE_SENSOR messages
void handle_msg(mavlink_message_t *msg);
void handle_msg(mavlink_message_t *msg) override;
private: