diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 1d04877134..ad189c22d4 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -268,9 +268,7 @@ AP_HAL::AnalogSource * batt_curr_pin; // SONAR selection //////////////////////////////////////////////////////////////////////////////// // -ModeFilterInt16_Size5 sonar_mode_filter(2); -AP_HAL::AnalogSource *sonar_analog_source; -AP_RangeFinder_MaxsonarXL *sonar; +static AP_RangeFinder_analog sonar; // relay support AP_Relay relay; @@ -401,8 +399,7 @@ static uint8_t receiver_rssi; // the time when the last HEARTBEAT message arrived from a GCS static uint32_t last_heartbeat_ms; -// The distance as reported by Sonar in cm – Values are 20 to 700 generally. -static int16_t sonar_dist_cm; +// Set to true when an obstacle is detected static bool obstacle = false; //////////////////////////////////////////////////////////////////////////////// @@ -580,16 +577,6 @@ void setup() { batt_volt_pin = hal.analogin->channel(g.battery_volt_pin); batt_curr_pin = hal.analogin->channel(g.battery_curr_pin); - #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC - sonar_analog_source = new AP_ADC_AnalogSource( - &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25); - #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN - sonar_analog_source = hal.analogin->channel( - CONFIG_SONAR_SOURCE_ANALOG_PIN); - sonar = new AP_RangeFinder_MaxsonarXL(sonar_analog_source, - &sonar_mode_filter); - #endif - init_ardupilot(); } @@ -668,7 +655,7 @@ static void fast_loop() // Read Sonar // ---------- if (g.sonar_enabled) { - sonar_dist_cm = sonar->read(); + float sonar_dist_cm = sonar.distance_cm(); if (sonar_dist_cm <= g.sonar_trigger_cm) { // obstacle detected in front obstacle = true; diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 844e74cfe6..38c9b3964a 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -420,6 +420,14 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan) hal.i2c->lockup_count()); } +static void NOINLINE send_rangefinder(mavlink_channel_t chan) +{ + mavlink_msg_rangefinder_send( + chan, + sonar.distance_cm() * 0.01, + sonar.voltage()); +} + static void NOINLINE send_current_waypoint(mavlink_channel_t chan) { mavlink_msg_mission_current_send( @@ -584,6 +592,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, send_hwstatus(chan); break; + case MSG_RANGEFINDER: + CHECK_PAYLOAD_SIZE(RANGEFINDER); + send_rangefinder(chan); + break; + case MSG_RETRY_DEFERRED: break; // just here to prevent a warning } @@ -862,6 +875,7 @@ GCS_MAVLINK::data_stream_send(void) if (stream_trigger(STREAM_EXTRA3)) { send_message(MSG_AHRS); send_message(MSG_HWSTATUS); + send_message(MSG_RANGEFINDER); } } diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index cff2a7c83e..18f76fd3e4 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -98,6 +98,7 @@ public: // obstacle control k_param_sonar_enabled = 190, + k_param_sonar, // sonar object k_param_sonar_type, k_param_sonar_trigger_cm, k_param_sonar_turn_angle, diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 911f8687ee..ecb752e591 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -356,6 +356,10 @@ const AP_Param::Info var_info[] PROGMEM = { GOBJECT(gcs0, "SR0_", GCS_MAVLINK), GOBJECT(gcs3, "SR3_", GCS_MAVLINK), + // @Group: SONAR_ + // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp + GOBJECT(sonar, "SONAR_", AP_RangeFinder_analog), + #if HIL_MODE == HIL_MODE_DISABLED // @Group: INS_ // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp diff --git a/APMrover2/defines.h b/APMrover2/defines.h index c3f9b39c37..a4b148b6fd 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -122,6 +122,7 @@ enum ap_message { MSG_AHRS, MSG_SIMSTATE, MSG_HWSTATUS, + MSG_RANGEFINDER, MSG_RETRY_DEFERRED // this must be last }; diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index e4014cbe89..7ec130db1a 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -1,13 +1,12 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + static void init_sonar(void) { - /* - #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC - sonar.calculate_scaler(g.sonar_type, 3.3); - #else - sonar.calculate_scaler(g.sonar_type, 5.0); - #endif -*/ +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 + sonar.Init(&adc); +#else + sonar.Init(NULL); +#endif } // Sensors are not available in HIL_MODE_ATTITUDE diff --git a/APMrover2/system.pde b/APMrover2/system.pde index 987873a063..bc6dce7732 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -180,8 +180,10 @@ static void init_ardupilot() //compass.get_offsets(); // load offsets to account for airframe magnetic interference } } + // initialise sonar - init_sonar(); + init_sonar(); + #endif // Do GPS init g_gps = &g_gps_driver; diff --git a/APMrover2/test.pde b/APMrover2/test.pde index d1e082a898..f063ca1ac0 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -551,14 +551,14 @@ test_sonar(uint8_t argc, const Menu::arg *argv) } print_hit_enter(); - delay(1000); - init_sonar(); - delay(1000); + init_sonar(); while (true) { - delay(20); - sonar_dist_cm = sonar->read(); - cliSerial->printf_P(PSTR("sonar distance = %u cm\n"), (unsigned)sonar_dist_cm); + delay(200); + float sonar_dist_cm = sonar.distance_cm(); + float voltage = sonar.voltage(); + cliSerial->printf_P(PSTR("sonar distance=%5.1fcm voltage=%5.2f\n"), + sonar_dist_cm, voltage); if (cliSerial->available() > 0) { break; }