mirror of https://github.com/ArduPilot/ardupilot
AP_RangeFinder: rename CompanionComputer to MAVLink
This commit is contained in:
parent
7960e3bb8f
commit
c541cb27f8
|
@ -14,61 +14,60 @@
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "AP_RangeFinder_CompanionComputer.h"
|
#include "AP_RangeFinder_MAVLink.h"
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
The constructor also initialises the rangefinder. Note that this
|
The constructor also initialises the rangefinder. Note that this
|
||||||
constructor is not called until detect() returns true, so we
|
constructor is not called until detect() returns true, so we
|
||||||
already know that we should setup the rangefinder
|
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)
|
AP_RangeFinder_Backend(_ranger, instance, _state)
|
||||||
{
|
{
|
||||||
last_update_ms = AP_HAL::millis();
|
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.
|
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,
|
// Assume that if the user set the RANGEFINDER_TYPE parameter to MAVLink,
|
||||||
//there is an attached companion computer rangefinder
|
// there is an attached MAVLink rangefinder
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Set the distance based on a MAVLINK message
|
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_distance_sensor_t packet;
|
||||||
mavlink_msg_distance_sensor_decode(msg, &packet);
|
mavlink_msg_distance_sensor_decode(msg, &packet);
|
||||||
|
|
||||||
last_update_ms = AP_HAL::millis();
|
last_update_ms = AP_HAL::millis();
|
||||||
cc_distance_cm = packet.current_distance;
|
distance_cm = packet.current_distance;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
update the state of the sensor
|
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
|
//Time out on incoming data; if we don't get new
|
||||||
//data in 500ms, dump it
|
//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);
|
set_status(RangeFinder::RangeFinder_NoData);
|
||||||
state.distance_cm = 0;
|
state.distance_cm = 0;
|
||||||
} else {
|
} else {
|
||||||
set_status(RangeFinder::RangeFinder_Good);
|
set_status(RangeFinder::RangeFinder_Good);
|
||||||
state.distance_cm = cc_distance_cm;
|
state.distance_cm = distance_cm;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -4,19 +4,15 @@
|
||||||
#include "RangeFinder.h"
|
#include "RangeFinder.h"
|
||||||
#include "RangeFinder_Backend.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
|
// 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:
|
public:
|
||||||
// constructor
|
// 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 detection function
|
||||||
static bool detect(RangeFinder &ranger, uint8_t instance);
|
static bool detect(RangeFinder &ranger, uint8_t instance);
|
||||||
|
@ -28,7 +24,7 @@ public:
|
||||||
void handle_msg(mavlink_message_t *msg);
|
void handle_msg(mavlink_message_t *msg);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
uint16_t cc_distance_cm;
|
uint16_t distance_cm;
|
||||||
uint32_t last_update_ms;
|
uint32_t last_update_ms;
|
||||||
|
|
||||||
// start a reading
|
// start a reading
|
||||||
|
|
|
@ -539,10 +539,10 @@ void RangeFinder::detect_instance(uint8_t instance)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
if (type == RangeFinder_TYPE_CC) {
|
if (type == RangeFinder_TYPE_MAVLink) {
|
||||||
if (AP_RangeFinder_CompanionComputer::detect(*this, instance)) {
|
if (AP_RangeFinder_MAVLink::detect(*this, instance)) {
|
||||||
state[instance].instance = 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;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -48,7 +48,7 @@ public:
|
||||||
RangeFinder_TYPE_LWI2C = 7,
|
RangeFinder_TYPE_LWI2C = 7,
|
||||||
RangeFinder_TYPE_LWSER = 8,
|
RangeFinder_TYPE_LWSER = 8,
|
||||||
RangeFinder_TYPE_BEBOP = 9,
|
RangeFinder_TYPE_BEBOP = 9,
|
||||||
RangeFinder_TYPE_CC = 10
|
RangeFinder_TYPE_MAVLink = 10
|
||||||
};
|
};
|
||||||
|
|
||||||
enum RangeFinder_Function {
|
enum RangeFinder_Function {
|
||||||
|
@ -107,7 +107,7 @@ public:
|
||||||
// 10Hz from main loop
|
// 10Hz from main loop
|
||||||
void update(void);
|
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);
|
void handle_msg(mavlink_message_t *msg);
|
||||||
|
|
||||||
#define _RangeFinder_STATE(instance) state[instance]
|
#define _RangeFinder_STATE(instance) state[instance]
|
||||||
|
|
Loading…
Reference in New Issue