diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_CompanionComputer.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_CompanionComputer.cpp
index ac780db94c..d013c18798 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_CompanionComputer.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_CompanionComputer.cpp
@@ -14,61 +14,60 @@
along with this program. If not, see .
*/
-#include "AP_RangeFinder_CompanionComputer.h"
+#include "AP_RangeFinder_MAVLink.h"
#include
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_CompanionComputer::AP_RangeFinder_CompanionComputer(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
+AP_RangeFinder_MAVLink::AP_RangeFinder_MAVLink(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
AP_RangeFinder_Backend(_ranger, instance, _state)
{
last_update_ms = AP_HAL::millis();
- cc_distance_cm = 0;
+ distance_cm = 0;
}
-/*
- detect if a CompanionComputer rangefinder is connected. We'll detect by
+/*
+ detect if a MAVLink rangefinder is connected. We'll detect by
checking a paramter.
*/
-bool AP_RangeFinder_CompanionComputer::detect(RangeFinder &_ranger, uint8_t instance)
+bool AP_RangeFinder_MAVLink::detect(RangeFinder &_ranger, uint8_t instance)
{
- //Assume that if the user set the RANGEFINDER_TYPE parameter to CompanionComputer,
- //there is an attached companion computer rangefinder
+ // Assume that if the user set the RANGEFINDER_TYPE parameter to MAVLink,
+ // there is an attached MAVLink rangefinder
return true;
}
/*
Set the distance based on a MAVLINK message
*/
-void AP_RangeFinder_CompanionComputer::handle_msg(mavlink_message_t *msg)
+void AP_RangeFinder_MAVLink::handle_msg(mavlink_message_t *msg)
{
mavlink_distance_sensor_t packet;
mavlink_msg_distance_sensor_decode(msg, &packet);
last_update_ms = AP_HAL::millis();
- cc_distance_cm = packet.current_distance;
+ distance_cm = packet.current_distance;
}
-/*
+/*
update the state of the sensor
*/
-void AP_RangeFinder_CompanionComputer::update(void)
+void AP_RangeFinder_MAVLink::update(void)
{
//Time out on incoming data; if we don't get new
//data in 500ms, dump it
- if(AP_HAL::millis() - last_update_ms > AP_RANGEFINDER_COMPANIONCOMPUTER_TIMEOUT_MS)
- {
+ if(AP_HAL::millis() - last_update_ms > AP_RANGEFINDER_MAVLINK_TIMEOUT_MS) {
set_status(RangeFinder::RangeFinder_NoData);
state.distance_cm = 0;
} else {
set_status(RangeFinder::RangeFinder_Good);
- state.distance_cm = cc_distance_cm;
+ state.distance_cm = distance_cm;
}
}
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_CompanionComputer.h b/libraries/AP_RangeFinder/AP_RangeFinder_CompanionComputer.h
index 085592d976..3c2e4cd88a 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_CompanionComputer.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_CompanionComputer.h
@@ -4,19 +4,15 @@
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
-// min and max distances
-#define AP_RANGEFINDER_COMPANIONCOMPUTER_MIN_DISTANCE 0
-#define AP_RANGEFINDER_COMPANIONCOMPUTER_MAX_DISTANCE 1400
-
// Data timeout
-#define AP_RANGEFINDER_COMPANIONCOMPUTER_TIMEOUT_MS 500
+#define AP_RANGEFINDER_MAVLINK_TIMEOUT_MS 500
-class AP_RangeFinder_CompanionComputer : public AP_RangeFinder_Backend
+class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend
{
public:
// constructor
- AP_RangeFinder_CompanionComputer(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
+ AP_RangeFinder_MAVLink(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
// static detection function
static bool detect(RangeFinder &ranger, uint8_t instance);
@@ -28,7 +24,7 @@ public:
void handle_msg(mavlink_message_t *msg);
private:
- uint16_t cc_distance_cm;
+ uint16_t distance_cm;
uint32_t last_update_ms;
// start a reading
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 79445b9971..dfa0223b9d 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -539,10 +539,10 @@ void RangeFinder::detect_instance(uint8_t instance)
}
}
#endif
- if (type == RangeFinder_TYPE_CC) {
- if (AP_RangeFinder_CompanionComputer::detect(*this, instance)) {
+ if (type == RangeFinder_TYPE_MAVLink) {
+ if (AP_RangeFinder_MAVLink::detect(*this, instance)) {
state[instance].instance = instance;
- drivers[instance] = new AP_RangeFinder_CompanionComputer(*this, instance, state[instance]);
+ drivers[instance] = new AP_RangeFinder_MAVLink(*this, instance, state[instance]);
return;
}
}
diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h
index 7e58f0af4d..44129b3729 100644
--- a/libraries/AP_RangeFinder/RangeFinder.h
+++ b/libraries/AP_RangeFinder/RangeFinder.h
@@ -48,7 +48,7 @@ public:
RangeFinder_TYPE_LWI2C = 7,
RangeFinder_TYPE_LWSER = 8,
RangeFinder_TYPE_BEBOP = 9,
- RangeFinder_TYPE_CC = 10
+ RangeFinder_TYPE_MAVLink = 10
};
enum RangeFinder_Function {
@@ -107,7 +107,7 @@ public:
// 10Hz from main loop
void update(void);
- // Handle an incoming message (for companion computer)
+ // Handle an incoming DISTANCE_SENSOR message (from a MAVLink enabled range finder)
void handle_msg(mavlink_message_t *msg);
#define _RangeFinder_STATE(instance) state[instance]