From 2fd9022f03e24a41ecb3dc002fc885c0b64559f2 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Fri, 28 Sep 2012 18:34:23 +0900 Subject: [PATCH] ArduCopter: bug fix to DMP initialisation (it was freezing due to SPI bus conflicts) --- .../AP_InertialSensor/AP_InertialSensor_MPU6000.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index af60530193..0056383fe9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -594,6 +594,11 @@ void AP_InertialSensor_MPU6000::dmp_init() return; } + // suspend timer + if( _scheduler != NULL ) { + _scheduler->suspend_timer(); + } + // load initial values into DMP memory dmp_load_mem(); @@ -640,6 +645,11 @@ void AP_InertialSensor_MPU6000::dmp_init() // dmp initialisation complete _dmp_initialised = true; + + // resume timer + if( _scheduler != NULL ) { + _scheduler->resume_timer(); + } } // dmp_reset - reset dmp (required for changes in gains or offsets to take effect)