Browse Source

ArduCopter: bug fix to DMP initialisation (it was freezing due to SPI bus conflicts)

mission-4.1.18
rmackay9 13 years ago
parent
commit
921ab3fe12
  1. 10
      libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

10
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@ -594,6 +594,11 @@ void AP_InertialSensor_MPU6000::dmp_init() @@ -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() @@ -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)

Loading…
Cancel
Save