diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 136ecff4e3..37de1895bb 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -393,6 +393,7 @@ setup_accel_scale(uint8_t argc, const Menu::arg *argv) ahrs.init(); ahrs.set_fly_forward(true); + ahrs.set_wind_estimation(true); ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 4f14fc1ac4..1613e50cf7 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -220,6 +220,7 @@ static void init_ardupilot() //read_EEPROM_airstart_critical(); ahrs.init(); ahrs.set_fly_forward(true); + ahrs.set_wind_estimation(true); ins.init(AP_InertialSensor::WARM_START, ins_sample_rate, @@ -448,6 +449,7 @@ static void startup_INS_ground(bool do_accel_init) ahrs.init(); ahrs.set_fly_forward(true); + ahrs.set_wind_estimation(true); ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 9212f8ae7a..ebb933e137 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -471,6 +471,7 @@ test_ins(uint8_t argc, const Menu::arg *argv) //cliSerial->printf_P(PSTR("Calibrating.")); ahrs.init(); ahrs.set_fly_forward(true); + ahrs.set_wind_estimation(true); ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, @@ -532,6 +533,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) } ahrs.init(); ahrs.set_fly_forward(true); + ahrs.set_wind_estimation(true); ahrs.set_compass(&compass); report_compass();