mirror of https://github.com/ArduPilot/ardupilot
Rover: always build sonar code
can use enabled/disabled parameter for runtime control
This commit is contained in:
parent
7b524d15fa
commit
06dd6c2790
|
@ -269,10 +269,8 @@ AP_HAL::AnalogSource * batt_curr_pin;
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
//
|
//
|
||||||
ModeFilterInt16_Size5 sonar_mode_filter(2);
|
ModeFilterInt16_Size5 sonar_mode_filter(2);
|
||||||
#if CONFIG_SONAR == ENABLED
|
AP_HAL::AnalogSource *sonar_analog_source;
|
||||||
AP_HAL::AnalogSource *sonar_analog_source;
|
AP_RangeFinder_MaxsonarXL *sonar;
|
||||||
AP_RangeFinder_MaxsonarXL *sonar;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// relay support
|
// relay support
|
||||||
AP_Relay relay;
|
AP_Relay relay;
|
||||||
|
@ -582,19 +580,15 @@ void setup() {
|
||||||
batt_volt_pin = hal.analogin->channel(g.battery_volt_pin);
|
batt_volt_pin = hal.analogin->channel(g.battery_volt_pin);
|
||||||
batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
|
batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
|
||||||
|
|
||||||
#if CONFIG_SONAR == ENABLED
|
|
||||||
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
|
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
|
||||||
sonar_analog_source = new AP_ADC_AnalogSource(
|
sonar_analog_source = new AP_ADC_AnalogSource(
|
||||||
&adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
|
&adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
|
||||||
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
|
#elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
|
||||||
sonar_analog_source = hal.analogin->channel(
|
sonar_analog_source = hal.analogin->channel(
|
||||||
CONFIG_SONAR_SOURCE_ANALOG_PIN);
|
CONFIG_SONAR_SOURCE_ANALOG_PIN);
|
||||||
#else
|
|
||||||
#warning "Invalid CONFIG_SONAR_SOURCE"
|
|
||||||
#endif
|
|
||||||
sonar = new AP_RangeFinder_MaxsonarXL(sonar_analog_source,
|
sonar = new AP_RangeFinder_MaxsonarXL(sonar_analog_source,
|
||||||
&sonar_mode_filter);
|
&sonar_mode_filter);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
init_ardupilot();
|
init_ardupilot();
|
||||||
}
|
}
|
||||||
|
@ -676,7 +670,6 @@ static void fast_loop()
|
||||||
#endif
|
#endif
|
||||||
// Read Sonar
|
// Read Sonar
|
||||||
// ----------
|
// ----------
|
||||||
#if CONFIG_SONAR == ENABLED
|
|
||||||
if(g.sonar_enabled){
|
if(g.sonar_enabled){
|
||||||
sonar_dist = sonar->read();
|
sonar_dist = sonar->read();
|
||||||
|
|
||||||
|
@ -686,7 +679,6 @@ static void fast_loop()
|
||||||
obstacle = false;
|
obstacle = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
// uses the yaw from the DCM to give more accurate turns
|
// uses the yaw from the DCM to give more accurate turns
|
||||||
calc_bearing_error();
|
calc_bearing_error();
|
||||||
|
|
|
@ -76,6 +76,7 @@
|
||||||
# define CONFIG_RELAY ENABLED
|
# define CONFIG_RELAY ENABLED
|
||||||
# define BATTERY_PIN_1 0
|
# define BATTERY_PIN_1 0
|
||||||
# define CURRENT_PIN_1 1
|
# define CURRENT_PIN_1 1
|
||||||
|
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ADC
|
||||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||||
# define CONFIG_INS_TYPE CONFIG_INS_MPU6000
|
# define CONFIG_INS_TYPE CONFIG_INS_MPU6000
|
||||||
# define CONFIG_PUSHBUTTON DISABLED
|
# define CONFIG_PUSHBUTTON DISABLED
|
||||||
|
@ -129,10 +130,6 @@
|
||||||
# define MAG_ORIENTATION ROTATION_NONE
|
# define MAG_ORIENTATION ROTATION_NONE
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
|
||||||
#define CONFIG_SONAR DISABLED
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// IMU Selection
|
// IMU Selection
|
||||||
//
|
//
|
||||||
|
@ -184,14 +181,6 @@
|
||||||
# define SONAR_ENABLED DISABLED
|
# define SONAR_ENABLED DISABLED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef CONFIG_SONAR
|
|
||||||
# define CONFIG_SONAR ENABLED
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef SONAR_TRIGGER
|
|
||||||
# define SONAR_TRIGGER 60 // trigger distance in cm
|
|
||||||
#endif
|
|
||||||
|
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
// HIL_MODE OPTIONAL
|
// HIL_MODE OPTIONAL
|
||||||
|
|
||||||
|
|
|
@ -1,5 +1,4 @@
|
||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
#if CONFIG_SONAR == ENABLED
|
|
||||||
static void init_sonar(void)
|
static void init_sonar(void)
|
||||||
{
|
{
|
||||||
/*
|
/*
|
||||||
|
@ -10,7 +9,6 @@ static void init_sonar(void)
|
||||||
#endif
|
#endif
|
||||||
*/
|
*/
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
#if LITE == DISABLED
|
#if LITE == DISABLED
|
||||||
// Sensors are not available in HIL_MODE_ATTITUDE
|
// Sensors are not available in HIL_MODE_ATTITUDE
|
||||||
|
|
|
@ -221,10 +221,7 @@ static void init_ardupilot()
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
// initialise sonar
|
// initialise sonar
|
||||||
#if CONFIG_SONAR == ENABLED
|
|
||||||
init_sonar();
|
init_sonar();
|
||||||
#endif
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
// Do GPS init
|
// Do GPS init
|
||||||
g_gps = &g_gps_driver;
|
g_gps = &g_gps_driver;
|
||||||
|
|
|
@ -16,9 +16,7 @@ static int8_t test_ins(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
||||||
#if CONFIG_SONAR == ENABLED
|
|
||||||
static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
|
||||||
#endif
|
|
||||||
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
|
||||||
static int8_t test_logging(uint8_t argc, const Menu::arg *argv);
|
static int8_t test_logging(uint8_t argc, const Menu::arg *argv);
|
||||||
|
@ -45,9 +43,7 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
|
||||||
#endif
|
#endif
|
||||||
{"gps", test_gps},
|
{"gps", test_gps},
|
||||||
{"ins", test_ins},
|
{"ins", test_ins},
|
||||||
#if CONFIG_SONAR == ENABLED
|
|
||||||
{"sonartest", test_sonar},
|
{"sonartest", test_sonar},
|
||||||
#endif
|
|
||||||
{"compass", test_mag},
|
{"compass", test_mag},
|
||||||
#elif HIL_MODE == HIL_MODE_SENSORS
|
#elif HIL_MODE == HIL_MODE_SENSORS
|
||||||
{"adc", test_adc},
|
{"adc", test_adc},
|
||||||
|
@ -546,28 +542,28 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
||||||
//-------------------------------------------------------------------------------------------
|
//-------------------------------------------------------------------------------------------
|
||||||
// real sensors that have not been simulated yet go here
|
// real sensors that have not been simulated yet go here
|
||||||
|
|
||||||
#if CONFIG_SONAR == ENABLED
|
|
||||||
static int8_t
|
static int8_t
|
||||||
test_sonar(uint8_t argc, const Menu::arg *argv)
|
test_sonar(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
print_hit_enter();
|
if (!g.sonar_enabled) {
|
||||||
|
cliSerial->println_P(PSTR("Sonar is not enabled"));
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
print_hit_enter();
|
||||||
delay(1000);
|
delay(1000);
|
||||||
init_sonar();
|
init_sonar();
|
||||||
delay(1000);
|
delay(1000);
|
||||||
|
|
||||||
while(1){
|
while (true) {
|
||||||
delay(20);
|
delay(20);
|
||||||
if(g.sonar_enabled){
|
sonar_dist = sonar->read();
|
||||||
sonar_dist = sonar->read();
|
cliSerial->printf_P(PSTR("sonar distance = %d cm\n"), (int)sonar_dist);
|
||||||
}
|
if (cliSerial->available() > 0) {
|
||||||
cliSerial->printf_P(PSTR("sonar_dist = %d\n"), (int)sonar_dist);
|
break;
|
||||||
|
|
||||||
if(cliSerial->available() > 0){
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return (0);
|
return (0);
|
||||||
}
|
}
|
||||||
#endif // SONAR == ENABLED
|
|
||||||
|
|
||||||
#endif // CLI_ENABLED
|
#endif // CLI_ENABLED
|
||||||
|
|
Loading…
Reference in New Issue