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]