diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 708d3d40de..1081b7dbab 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -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; diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index eb0adcfd24..d7b92b4a3b 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -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( diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index a51e2c3a1e..effbb4baf7 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -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, diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index c9650c424e..205baf1dd0 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -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 diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 19d66f3004..d96a6d4f09 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -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 diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 05613784be..cf3b818f43 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -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 diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index 52c2d92f7f..0a01c4aea1 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -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 diff --git a/APMrover2/test.pde b/APMrover2/test.pde index c1a173f676..977f1b069a 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -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;