AP_HAL_FLYMAPLE: Sensor reading improvements

I2C speed increased to 400kHz
Accelerometer 8g max, Normal power, 800Hz bandwidth
Gyros: 2000 degrees/sec, 256Hz LPF, 8kHz internal sample rate

libraries/AP_HAL_FLYMAPLE/FlymaplePortingNotes.txt

@@ -46,9 +46,6 @@ In other parts of the ArduPlane tree, the changes have been #ifdefed for
 Flymaple except:
 
 - ArduPlane/compat.pde
-All the wiring compatibility functions have been inline-d, else get
-contamination of the namespace at link time (multiple definitions of delay()
-etc)
 
 - libraries/AP_Compass/AP_Compass_HMC5843.cpp
 - libraries/AP_Compass/Compass.h
@@ -58,6 +55,8 @@ affect other platforms.
 
 Some other minor edits to eliminate compiler warnings
 
+These changes have now all been included in the ardupilot mainline code.
+
 Resource usage
 Resources on the Flymaple board have been allocated by the HAL:
 
@@ -66,11 +65,13 @@ Pins
         GPS. 3.3V only, NOT 5V tolerant
 1       AP GPS on Flymaple Serial2  Tx out. This is where you connect the GPS.
 	3.3V
+5       I2C SCL. Do not use for GPIO.
 6	Receiver PPM in
 7	Console and Mavlink on Flymaple Serial1 Rx in. Also on connector
 	"COM1". 5V tolerant.
 8	Console and Mavlink on Flymaple Serial1 Tx out. Also on connector
 	"COM1". 3.3V
+9       I2C SDA. Do not use for GPIO
 15	3.3V board VCC analog in. Connect to 3.3V pin.
 16	Airspeed analog in (if available). 3.3V, NOT 5V tolerant.
 19	Battery current analog in (if available). 3.3V, NOT 5V tolerant.
@@ -85,9 +86,29 @@ SysTick	1000Hz normal timers
 3,4	RCOut
 8	not used by AP
 
+AP I2CDriver on Flymaple uses the libmaple Wire library, configured to clock
+at about 200kHz. The maximum possible Flymaple could support speed is limited
+by the BMP085 barometer to 400kHz.
+Current version of the Wire library is unfortunately
+bit-banged. This may change in the future.
+
+At 200kHz I2C speed, it takes 500us to read the 6 byte accelerometer
+buffer, and 500us to read the 6 byte gyro buffer.
+
 The SerialUSB (USB connection) to Flymaple is not used by AP. It can be used for
 debugging inside AP_HAL_FLYMAPLE, using SerialUSB.println().
 
+Sensor configuration
+
+The sensors are configured so:
+
+ADXL345 Accelerometer
+8g full scale, full resolution mode, 800Hz bandwidth, read at 1kHz sample rate
+per sec
+
+ITG3205 Gyro
+2000 degrees/sec, 256Hz LPF, 8kHz internal sample rate, read at 1kHz sample rate
+
 Installation on Linux
 
 Tested with:

libraries/AP_HAL_FLYMAPLE/I2CDriver.cpp

@@ -27,7 +27,7 @@
 
 using namespace AP_HAL_FLYMAPLE_NS;
 
-static TwoWire twowire(5, 9); // Flymaple has non-standard SCL, SDA
+static TwoWire twowire(5, 9, SOFT_FAST); // Flymaple has non-standard SCL, SDA, speed ~200kHz
 void FLYMAPLEI2CDriver::begin() 
 {
     twowire.begin();

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp

@@ -18,8 +18,8 @@
  */
 
 // Interface to the Flymaple sensors:
-// ITG3205 Gyroscope
+// ITG3205 Gyroscope  http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf
-// ADXL345 Accelerometer
+// ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
 
 #include <AP_HAL.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
@@ -39,7 +39,7 @@ int AP_InertialSensor_Flymaple::_accel_fd;
 int AP_InertialSensor_Flymaple::_gyro_fd;
 
 ///////
-/// Accelerometer ADXL345 definitions
+/// Accelerometer ADXL345 register definitions
 #define FLYMAPLE_ACCELEROMETER_ADDRESS              0x53
 #define FLYMAPLE_ACCELEROMETER_XL345_DEVID          0xe5
 #define FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE      0x2c
@@ -49,14 +49,25 @@ int AP_InertialSensor_Flymaple::_gyro_fd;
 #define FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0       0x32
 #define FLYMAPLE_ACCELEROMETER_GRAVITY              248
 
-/// Gyro ITG3205 definitions
+// ADXL345 accelerometer scaling
+// Result will be scaled to 1m/s/s
+// ADXL345 in Full resolution mode (any g scaling) is 256 counts/g, so scale by 9.81/256 = 0.038320312
+#define FLYMAPLE_ACCELEROMETER_SCALE_M_S    (GRAVITY_MSS / 256.0f)
+
+/// Gyro ITG3205 register definitions
 #define FLYMAPLE_GYRO_ADDRESS                       0x68
+#define FLYMAPLE_GYRO_WHO_AM_I                      0x00
 #define FLYMAPLE_GYRO_PWR_MGM                       0x3e
 #define FLYMAPLE_GYRO_DLPF_FS                       0x16
 #define FLYMAPLE_GYRO_INT_CFG                       0x17
 #define FLYMAPLE_GYRO_SMPLRT_DIV                    0x15
 #define FLYMAPLE_GYRO_GYROX_H                       0x1d
 
+// ITG3200 Gyroscope scaling
+// ITG3200 is 14.375 LSB degrees/sec with FS_SEL=3
+// Result wil be radians/sec
+#define FLYMAPLE_GYRO_SCALE_R_S (1.0f / 14.375f) * (3.1415926f / 180.0f)
+
 uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate ) 
 {
     switch (sample_rate) {
@@ -84,23 +95,40 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
     hal.scheduler->delay(5);
     hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0xff);
     hal.scheduler->delay(5);
+    // Measure mode:
     hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0x08);
     hal.scheduler->delay(5);
+    // Full resolution, 8g:
+    // Caution, this must agree with FLYMAPLE_ACCELEROMETER_SCALE_1G
+    // In full resoution mode, the scale factor need not change
     hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATA_FORMAT, 0x08);
     hal.scheduler->delay(5);
-    hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE, 0x09);
+    // Normal power, 800Hz bandwidth:
+    hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE, 0x0e);
     hal.scheduler->delay(5);
+    // Power up default is FIFO bypass mode. FIFO is not used by the chip
 
-    /// Init the Gyro
+    // Init the Gyro
+    // Expect to read the same as the Gyro I2C adress:
+    hal.i2c->readRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_WHO_AM_I, &data);
+    if (data != FLYMAPLE_GYRO_ADDRESS)
+        hal.scheduler->panic(PSTR("AP_InertialSensor_Flymaple: could not find ITG-3200 accelerometer sensor"));
     hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_PWR_MGM, 0x00);
     hal.scheduler->delay(1);
-    hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_SMPLRT_DIV, 0x07);
+    // Sample rate divider: with 8kHz internal clock (see FLYMAPLE_GYRO_DLPF_FS), 
+    // get 500Hz sample rate, 2 samples
+    hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_SMPLRT_DIV, 0x0f);
     hal.scheduler->delay(1);
-    hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_DLPF_FS,0x1e);
+    // 2000 degrees/sec, 256Hz LPF, 8kHz internal sample rate
+    hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_DLPF_FS, 0x18);
     hal.scheduler->delay(1);
+    // No interrupts
     hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_INT_CFG, 0x00);
     hal.scheduler->delay(1);
 
+// FIXME:
+hal.gpio->pinMode(4, GPIO_OUTPUT);
+
     return AP_PRODUCT_ID_FLYMAPLE;
 }
 
@@ -144,13 +172,20 @@ bool AP_InertialSensor_Flymaple::update(void)
 
     // add offsets and rotation
     _accel.rotate(_board_orientation);
+
+    // Adjust for chip scaling to get m/s/s
+    _accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
+
+    // Now the calibration scale factor
     _accel.x *= accel_scale.x;
     _accel.y *= accel_scale.y;
     _accel.z *= accel_scale.z;
     _accel   -= _accel_offset;
 
     _gyro.rotate(_board_orientation);
-    _gyro *= (1.0 / 14.375) * (3.1415926 / 180); // ITG3200 14.375 LSB degrees/sec with FS_SEL=3
+
+    // Adjust for chip scaling to get radians/sec
+    _gyro *= FLYMAPLE_GYRO_SCALE_R_S;
     _gyro -= _gyro_offset;
 
 #if 0
@@ -175,6 +210,7 @@ uint32_t AP_InertialSensor_Flymaple::get_last_sample_time_micros(void)
 
 float AP_InertialSensor_Flymaple::get_gyro_drift_rate(void) 
 {
+    // Dont really know this for the ITG-3200.
     // 0.5 degrees/second/minute
     return ToRad(0.5/60);
 }
@@ -187,8 +223,10 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
     _in_accumulate = true;
 
     // Read accelerometer
-    uint8_t buffer[8];
+    // ADXL345 is in the default FIFO bypass mode, so the FIFO is not used
-    if (hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 8, buffer) == 0)
+    uint8_t buffer[6];
+    // This takes about 500us at 400kHz I2C speed
+    if (hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0)
     {
         // The order is a bit wierd here since the standard we have adopted for Flymaple 
         // sensor orientation is different to what the board designers intended
@@ -203,6 +241,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
     }
 
     // Read gyro
+    // This takes about 500us at 400kHz I2C speed
     if (hal.i2c->readRegisters(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_GYROX_H, 6, buffer) == 0)
     {
         int16_t y = -((((int16_t)buffer[0]) << 8) | buffer[1]);    // chip X axis
@@ -212,15 +251,12 @@ void AP_InertialSensor_Flymaple::_accumulate(void)
         _gyro_sum_count++;
         _last_gyro_timestamp = hal.scheduler->micros();
     }
+// FIXME:
+hal.gpio->write(4, 0);
 
     _in_accumulate = false;
 }
 
-void AP_InertialSensor_Flymaple::_ins_timer(uint32_t now)
-{
-    _accumulate();
-}
-
 bool AP_InertialSensor_Flymaple::sample_available(void)
 {
     _accumulate();

libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h

@@ -24,7 +24,6 @@ public:
 
 private:
     uint16_t        _init_sensor( Sample_rate sample_rate );
-    static		    void _ins_timer(uint32_t now);
     static          void _accumulate(void);
     uint64_t        _last_update_usec;
     float           _delta_time;