HAL_AVR: set initial bus speed on MPU6k to 500kHz, then change to 8MHz

this should allow us to work even with older chips. See the MPU6000
product specification, which says max 20MHz for sensor and interrupt
regs, and max 1MHz for all other regs

libraries/AP_HAL_AVR/SPIDeviceManager_APM1.cpp

@@ -17,14 +17,14 @@ void APM1SPIDeviceManager::init(void* machtnichts) {
     /* dataflash: divide clock by 2 to 8Mhz, set SPI_MODE_3
      * spcr gets 0x0C to set SPI_MODE_3
      * spsr gets bit SPI2X for clock divider */
-    _dataflash = new AVRSPI0DeviceDriver(df_cs, 0x0C, _BV(SPI2X));
+    _dataflash = new AVRSPI0DeviceDriver(df_cs, 0x0C, 0x0C, _BV(SPI2X));
     _dataflash->init();
 
     /* optflow cs is on Arduino pin 34, PORTC3 */
     AVRDigitalSource* opt_cs = new AVRDigitalSource(_BV(3), PC);
     /* optflow: divide clock by 8 to 2Mhz
      * spcr gets bit SPR0, spsr gets bit SPI2X */
-    _optflow = new AVRSPI0DeviceDriver(opt_cs, _BV(SPR0), 0);
+    _optflow = new AVRSPI0DeviceDriver(opt_cs, _BV(SPR0), _BV(SPR0), 0);
     _optflow->init();
 
     /* adc cs is on Arduino pin 33, PORTC4 */

libraries/AP_HAL_AVR/SPIDeviceManager_APM2.cpp

@@ -22,21 +22,22 @@ void APM2SPIDeviceManager::init(void* machtnichts) {
 
     /* mpu6k cs is on Arduino pin 53, PORTB0 */
     AVRDigitalSource* mpu6k_cs = new AVRDigitalSource(_BV(0), PB);
-    /* mpu6k: run clock at 8MHz */
-    _mpu6k = new AVRSPI0DeviceDriver(mpu6k_cs, 0, _BV(SPI2X));
+    /* mpu6k: run clock at 8MHz in high speed mode and 512kHz for low
+     * speed */
+    _mpu6k = new AVRSPI0DeviceDriver(mpu6k_cs, _BV(SPR1), 0, _BV(SPI2X));
     _mpu6k->init();
 
     /* ms5611 cs is on Arduino pin 40, PORTG1 */
     AVRDigitalSource* ms5611_cs = new AVRDigitalSource(_BV(1), PG);
-    /* mpu6k: run clock at 8MHz */
-    _ms5611 = new AVRSPI0DeviceDriver(ms5611_cs, 0, _BV(SPI2X));
+    /* ms5611: run clock at 8MHz */
+    _ms5611 = new AVRSPI0DeviceDriver(ms5611_cs, 0, 0, _BV(SPI2X));
     _ms5611->init();
    
     /* optflow cs is on Arduino pin A3, PORTF3 */
     AVRDigitalSource* optflow_cs = new AVRDigitalSource(_BV(3), PF);
     /* optflow: divide clock by 8 to 2Mhz
      * spcr gets bit SPR0, spsr gets bit SPI2X */
-    _optflow_spi0 = new AVRSPI0DeviceDriver(optflow_cs, _BV(SPR0), _BV(SPI2X));
+    _optflow_spi0 = new AVRSPI0DeviceDriver(optflow_cs, _BV(SPR0), _BV(SPR0), _BV(SPI2X));
     _optflow_spi0->init();
 
     /* Dataflash CS is on Arduino pin 28, PORTA6 */

libraries/AP_HAL_AVR/SPIDevice_SPI0.cpp

@@ -134,4 +134,16 @@ uint8_t AVRSPI0DeviceDriver::transfer(uint8_t data) {
     return _transfer(data);
 }
 
+/**
+   allow on the fly bus speed changes for MPU6000
+ */
+void AVRSPI0DeviceDriver::set_bus_speed(AVRSPI0DeviceDriver::bus_speed speed) 
+{
+    if (speed == AVRSPI0DeviceDriver::SPI_SPEED_HIGH) {
+        _spcr = _spcr_highspeed;
+    } else {
+        _spcr = _spcr_lowspeed;
+    }
+}
+
 #endif

libraries/AP_HAL_AVR/SPIDevices.h

@@ -9,11 +9,14 @@ class AP_HAL_AVR::AVRSPI0DeviceDriver : public AP_HAL::SPIDeviceDriver {
 public:
     AVRSPI0DeviceDriver(
             AP_HAL_AVR::AVRDigitalSource *cs_pin,
-            uint8_t spcr,
+            uint8_t spcr_lowspeed,
+            uint8_t spcr_highspeed,
             uint8_t spsr
     ) :
         _cs_pin(cs_pin),
-        _spcr(spcr),
+        _spcr(spcr_lowspeed),
+        _spcr_lowspeed(spcr_lowspeed),
+        _spcr_highspeed(spcr_highspeed),
         _spsr(spsr)
     {}
 
@@ -26,6 +29,7 @@ public:
     void cs_release();
     uint8_t transfer(uint8_t data);
     void transfer(const uint8_t *data, uint16_t len);
+    void set_bus_speed(enum bus_speed speed);
 
 private:
     void _cs_assert();
@@ -37,7 +41,9 @@ private:
     static AP_HAL_AVR::AVRSemaphore _semaphore;
 
     AP_HAL_AVR::AVRDigitalSource *_cs_pin;
-    const uint8_t _spcr;
+    const uint8_t _spcr_lowspeed;
+    const uint8_t _spcr_highspeed;
+    uint8_t _spcr;
     const uint8_t _spsr;
 };