diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 38ab52a235..91f7f8ea8c 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -1,12 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - #if CONFIG_SONAR == ENABLED -static void init_sonar(void) -{ - sonar.init(); -} -#endif - static void init_barometer(bool full_calibration) { gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer")); @@ -41,6 +34,13 @@ static void read_barometer(void) } } +#if CONFIG_SONAR == ENABLED +static void init_sonar(void) +{ + sonar.init(); +} +#endif + // return sonar altitude in centimeters static int16_t read_sonar(void) { @@ -77,6 +77,7 @@ static int16_t read_sonar(void) #endif } +// initialise compass static void init_compass() { if (!compass.init() || !compass.read()) { @@ -88,6 +89,7 @@ static void init_compass() ahrs.set_compass(&compass); } +// initialise optical flow sensor static void init_optflow() { #if OPTFLOW == ENABLED