Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: convert to new AP_RangeFinder API

Browse Source
This commit is contained in:
Andrew Tridgell 2014-06-27 13:18:20 +10:00
parent 28e1449e8d
commit bfe705a14d
8 changed files with 31 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
APMrover2/APMrover2.pde
View File

@ -298,11 +298,8 @@ static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
AP_HAL::AnalogSource *rssi_analog_source;
////////////////////////////////////////////////////////////////////////////////
// SONAR selection
////////////////////////////////////////////////////////////////////////////////
//
static AP_RangeFinder_analog sonar;
static AP_RangeFinder_analog sonar2;
// SONAR
static RangeFinder sonar;
// relay support
AP_Relay relay;

16
APMrover2/GCS_Mavlink.pde
View File

@ -389,7 +389,7 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
static void NOINLINE send_rangefinder(mavlink_channel_t chan)
{
if (!sonar.enabled()) {
if (!sonar.healthy()) {
// no sonar to report
return;
}
@ -398,18 +398,18 @@ static void NOINLINE send_rangefinder(mavlink_channel_t chan)
report smaller distance of two sonars if more than one enabled
*/
float distance_cm, voltage;
if (!sonar2.enabled()) {
distance_cm = sonar.distance_cm();
voltage = sonar.voltage();
if (!sonar.healthy(1)) {
distance_cm = sonar.distance_cm(0);
voltage = sonar.voltage_mv(0) * 0.001f;
} else {
float dist1 = sonar.distance_cm();
float dist2 = sonar2.distance_cm();
float dist1 = sonar.distance_cm(0);
float dist2 = sonar.distance_cm(1);
if (dist1 <= dist2) {
distance_cm = dist1;
voltage = sonar.voltage();
voltage = sonar.voltage_mv(0) * 0.001f;
} else {
distance_cm = dist2;
voltage = sonar2.voltage();
voltage = sonar.voltage_mv(1) * 0.001f;
}
}
mavlink_msg_rangefinder_send(

4
APMrover2/Log.pde
View File

@ -368,8 +368,8 @@ static void Log_Write_Sonar()
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
time_ms : millis(),
lateral_accel : lateral_acceleration,
sonar1_distance : (uint16_t)sonar.distance_cm(),
sonar2_distance : (uint16_t)sonar2.distance_cm(),
sonar1_distance : (uint16_t)sonar.distance_cm(0),
sonar2_distance : (uint16_t)sonar.distance_cm(1),
detected_count : obstacle.detected_count,
turn_angle : (int8_t)obstacle.turn_angle,
turn_time : turn_time,

5
APMrover2/Parameters.h
View File

@ -129,12 +129,13 @@ public:
// obstacle control
k_param_sonar_enabled = 190, // deprecated, can be removed
k_param_sonar, // sonar object
k_param_sonar_old, // unused
k_param_sonar_trigger_cm,
k_param_sonar_turn_angle,
k_param_sonar_turn_time,
k_param_sonar2, // sonar2 object
k_param_sonar2_old, // unused
k_param_sonar_debounce,
k_param_sonar, // sonar object
//
// 210: driving modes

10
APMrover2/Parameters.pde
View File

@ -482,13 +482,9 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
GOBJECT(L1_controller, "NAVL1_", AP_L1_Control),
// @Group: SONAR_
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
GOBJECT(sonar, "SONAR_", AP_RangeFinder_analog),
// @Group: SONAR2_
// @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
GOBJECT(sonar2, "SONAR2_", AP_RangeFinder_analog),
// @Group: SONAR
// @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
GOBJECT(sonar, "SONAR", RangeFinder),
// @Group: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp

1
APMrover2/defines.h
View File

@ -71,7 +71,6 @@ enum mode {
#define LOG_ATTITUDE_MSG 0x08
#define LOG_MODE_MSG 0x09
#define LOG_COMPASS_MSG 0x0A
#define LOG_CAMERA_MSG 0x0B // deprecated
#define LOG_COMPASS2_MSG 0x0C
#define LOG_STEERING_MSG 0x0D
#define LOG_COMPASS3_MSG 0x0E

20
APMrover2/sensors.pde
View File

@ -9,13 +9,7 @@ static void init_barometer(void)
static void init_sonar(void)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
sonar.Init(&adc);
sonar2.Init(&adc);
#else
sonar.Init(NULL);
sonar2.Init(NULL);
#endif
sonar.init();
}
// read_battery - reads battery voltage and current and invokes failsafe
@ -37,15 +31,17 @@ void read_receiver_rssi(void)
// read the sonars
static void read_sonars(void)
{
if (!sonar.enabled()) {
sonar.update();
if (!sonar.healthy()) {
// this makes it possible to disable sonar at runtime
return;
}
if (sonar2.enabled()) {
if (sonar.healthy(1)) {
// we have two sonars
obstacle.sonar1_distance_cm = sonar.distance_cm();
obstacle.sonar2_distance_cm = sonar2.distance_cm();
obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = sonar.distance_cm(1);
if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm &&
obstacle.sonar2_distance_cm <= (uint16_t)obstacle.sonar2_distance_cm) {
// we have an object on the left
@ -72,7 +68,7 @@ static void read_sonars(void)
}
} else {
// we have a single sonar
obstacle.sonar1_distance_cm = sonar.distance_cm();
obstacle.sonar1_distance_cm = sonar.distance_cm(0);
obstacle.sonar2_distance_cm = 0;
if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm) {
// obstacle detected in front

10
APMrover2/test.pde
View File

@ -455,7 +455,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
static int8_t
test_sonar(uint8_t argc, const Menu::arg *argv)
{
if (!sonar.enabled()) {
if (!sonar.healthy()) {
cliSerial->println_P(PSTR("WARNING: Sonar is not enabled"));
}
@ -474,8 +474,8 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
delay(20);
uint32_t now = millis();
float dist_cm = sonar.distance_cm();
float voltage = sonar.voltage();
float dist_cm = sonar.distance_cm(0);
float voltage = sonar.voltage_mv(0);
if (sonar_dist_cm_min == 0.0f) {
sonar_dist_cm_min = dist_cm;
voltage_min = voltage;
@ -485,8 +485,8 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
voltage_min = min(voltage_min, voltage);
voltage_max = max(voltage_max, voltage);
dist_cm = sonar2.distance_cm();
voltage = sonar2.voltage();
dist_cm = sonar.distance_cm(1);
voltage = sonar.voltage_mv(1);
if (sonar2_dist_cm_min == 0.0f) {
sonar2_dist_cm_min = dist_cm;
voltage2_min = voltage;