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

Rover: use rangefinder backend accessors

This commit is contained in:
Peter Barker 2017-08-08 15:58:52 +10:00 committed by Francisco Ferreira
parent e0bea597c0
commit 4c2747bfe8
3 changed files with 39 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
APMrover2/GCS_Mavlink.cpp
View File

@ -3,6 +3,8 @@
#include "GCS_Mavlink.h"
#include <AP_RangeFinder/RangeFinder_Backend.h>
void Rover::send_heartbeat(mavlink_channel_t chan)
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
@ -184,35 +186,26 @@ void Rover::send_simstate(mavlink_channel_t chan)
void Rover::send_rangefinder(mavlink_channel_t chan)
{
if (!rangefinder.has_data(0) && !rangefinder.has_data(1)) {
// no rangefinder to report
return;
float distance_cm;
float voltage;
bool got_one = false;
// report smaller distance of all rangefinders
for (uint8_t i=0; i<rangefinder.num_sensors(); i++) {
AP_RangeFinder_Backend *s = rangefinder.get_backend(i);
if (s == nullptr) {
continue;
}
if (!got_one ||
s->distance_cm() < distance_cm) {
distance_cm = s->distance_cm();
voltage = s->voltage_mv();
got_one = true;
}
}
float distance_cm = 0.0f;
float voltage = 0.0f;
/*
report smaller distance of two rangefinders
*/
if (rangefinder.has_data(0) && rangefinder.has_data(1)) {
if (rangefinder.distance_cm(0) <= rangefinder.distance_cm(1)) {
distance_cm = rangefinder.distance_cm(0);
voltage = rangefinder.voltage_mv(0);
} else {
distance_cm = rangefinder.distance_cm(1);
voltage = rangefinder.voltage_mv(1);
}
} else {
// only rangefinder 0 or rangefinder 1 has data
if (rangefinder.has_data(0)) {
distance_cm = rangefinder.distance_cm(0);
voltage = rangefinder.voltage_mv(0) * 0.001f;
}
if (rangefinder.has_data(1)) {
distance_cm = rangefinder.distance_cm(1);
voltage = rangefinder.voltage_mv(1) * 0.001f;
}
if (!got_one) {
// no relevant data found
return;
}
mavlink_msg_rangefinder_send(

8
APMrover2/Log.cpp
View File

@ -1,6 +1,8 @@
#include "Rover.h"
#include "version.h"
#include <AP_RangeFinder/RangeFinder_Backend.h>
#if LOGGING_ENABLED == ENABLED
struct PACKED log_Performance {
@ -178,12 +180,14 @@ void Rover::Log_Write_Rangefinder()
if (!is_zero(obstacle.turn_angle)) {
turn_time = AP_HAL::millis() - obstacle.detected_time_ms;
}
AP_RangeFinder_Backend *s0 = rangefinder.get_backend(0);
AP_RangeFinder_Backend *s1 = rangefinder.get_backend(1);
struct log_Rangefinder pkt = {
LOG_PACKET_HEADER_INIT(LOG_RANGEFINDER_MSG),
time_us : AP_HAL::micros64(),
lateral_accel : control_mode->lateral_acceleration,
rangefinder1_distance : rangefinder.distance_cm(0),
rangefinder2_distance : rangefinder.distance_cm(1),
rangefinder1_distance : s0 ? s0->distance_cm() : (uint16_t)0,
rangefinder2_distance : s1 ? s1->distance_cm() : (uint16_t)0,
detected_count : obstacle.detected_count,
turn_angle : static_cast<int8_t>(obstacle.turn_angle),
turn_time : turn_time,

18
APMrover2/sensors.cpp
View File

@ -1,5 +1,7 @@
#include "Rover.h"
#include <AP_RangeFinder/RangeFinder_Backend.h>
// initialise compass
void Rover::init_compass()
{
@ -200,15 +202,18 @@ void Rover::read_rangefinders(void)
{
rangefinder.update();
if (rangefinder.status(0) == RangeFinder::RangeFinder_NotConnected) {
AP_RangeFinder_Backend *s0 = rangefinder.get_backend(0);
AP_RangeFinder_Backend *s1 = rangefinder.get_backend(1);
if (s0 == nullptr || s0->status() == RangeFinder::RangeFinder_NotConnected) {
// this makes it possible to disable rangefinder at runtime
return;
}
if (rangefinder.has_data(1)) {
if (s1 != nullptr && s1->has_data()) {
// we have two rangefinders
obstacle.rangefinder1_distance_cm = rangefinder.distance_cm(0);
obstacle.rangefinder2_distance_cm = rangefinder.distance_cm(1);
obstacle.rangefinder1_distance_cm = s0->distance_cm();
obstacle.rangefinder2_distance_cm = s1->distance_cm();
if (obstacle.rangefinder1_distance_cm < static_cast<uint16_t>(g.rangefinder_trigger_cm) &&
obstacle.rangefinder1_distance_cm < static_cast<uint16_t>(obstacle.rangefinder2_distance_cm)) {
// we have an object on the left
@ -235,7 +240,7 @@ void Rover::read_rangefinders(void)
}
} else {
// we have a single rangefinder
obstacle.rangefinder1_distance_cm = rangefinder.distance_cm(0);
obstacle.rangefinder1_distance_cm = s0->distance_cm();
obstacle.rangefinder2_distance_cm = 0;
if (obstacle.rangefinder1_distance_cm < static_cast<uint16_t>(g.rangefinder_trigger_cm)) {
// obstacle detected in front
@ -347,7 +352,8 @@ void Rover::update_sensor_status_flags(void)
if (g.rangefinder_trigger_cm > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
if (rangefinder.has_data(0)) {
AP_RangeFinder_Backend *s = rangefinder.get_backend(0);
if (s != nullptr && s->has_data()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}