diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 58a9fe6b88..31c634afa6 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -189,5 +189,6 @@ protected:
#include "AP_InertialSensor_PX4.h"
#include "AP_InertialSensor_UserInteract_Stream.h"
#include "AP_InertialSensor_UserInteract_MAVLink.h"
+#include "AP_InertialSensor_Flymaple.h"
#endif // __AP_INERTIAL_SENSOR_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
new file mode 100644
index 0000000000..e46d2d2080
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
@@ -0,0 +1,227 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ Flymaple port by Mike McCauley
+ */
+
+// Interface to the Flymaple sensors:
+// ITG3205 Gyroscope
+// ADXL345 Accelerometer
+
+#include
+#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
+#include "AP_InertialSensor_Flymaple.h"
+
+const extern AP_HAL::HAL& hal;
+
+
+Vector3f AP_InertialSensor_Flymaple::_accel_sum;
+uint32_t AP_InertialSensor_Flymaple::_accel_sum_count;
+Vector3f AP_InertialSensor_Flymaple::_gyro_sum;
+uint32_t AP_InertialSensor_Flymaple::_gyro_sum_count;
+volatile bool AP_InertialSensor_Flymaple::_in_accumulate;
+uint64_t AP_InertialSensor_Flymaple::_last_accel_timestamp;
+uint64_t AP_InertialSensor_Flymaple::_last_gyro_timestamp;
+int AP_InertialSensor_Flymaple::_accel_fd;
+int AP_InertialSensor_Flymaple::_gyro_fd;
+
+///////
+/// Accelerometer ADXL345 definitions
+#define FLYMAPLE_ACCELEROMETER_ADDRESS 0x53
+#define FLYMAPLE_ACCELEROMETER_XL345_DEVID 0xe5
+#define FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE 0x2c
+#define FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL 0x2d
+#define FLYMAPLE_ACCELEROMETER_ADXLREG_DATA_FORMAT 0x31
+#define FLYMAPLE_ACCELEROMETER_ADXLREG_DEVID 0x00
+#define FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0 0x32
+#define FLYMAPLE_ACCELEROMETER_GRAVITY 248
+
+/// Gyro ITG3205 definitions
+#define FLYMAPLE_GYRO_ADDRESS 0x68
+#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
+
+uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate )
+{
+ switch (sample_rate) {
+ case RATE_50HZ:
+ _sample_divider = 4;
+ _default_filter_hz = 10;
+ break;
+ case RATE_100HZ:
+ _sample_divider = 2;
+ _default_filter_hz = 20;
+ break;
+ case RATE_200HZ:
+ default:
+ _sample_divider = 1;
+ _default_filter_hz = 20;
+ break;
+ }
+
+ // Init the accelerometer
+ uint8_t data;
+ hal.i2c->readRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DEVID, &data);
+ if (data != FLYMAPLE_ACCELEROMETER_XL345_DEVID)
+ hal.scheduler->panic(PSTR("AP_InertialSensor_Flymaple: could not find ADXL345 accelerometer sensor"));
+ hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0x00);
+ hal.scheduler->delay(5);
+ hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0xff);
+ hal.scheduler->delay(5);
+ hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0x08);
+ hal.scheduler->delay(5);
+ 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);
+ hal.scheduler->delay(5);
+
+ /// Init the Gyro
+ 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);
+ hal.scheduler->delay(1);
+ hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_DLPF_FS,0x1e);
+ hal.scheduler->delay(1);
+ hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_INT_CFG, 0x00);
+ hal.scheduler->delay(1);
+
+ return AP_PRODUCT_ID_FLYMAPLE;
+}
+
+/*
+ set the filter frequency
+ */
+void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
+{
+ if (filter_hz == 0) {
+ filter_hz = _default_filter_hz;
+ }
+
+/// TODO ...
+}
+
+/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
+
+bool AP_InertialSensor_Flymaple::update(void)
+{
+ while (num_samples_available() == 0) {
+ hal.scheduler->delay(1);
+ }
+ Vector3f accel_scale = _accel_scale.get();
+
+ hal.scheduler->suspend_timer_procs();
+
+ // base the time on the gyro timestamp, as that is what is
+ // multiplied by time to integrate in DCM
+ _delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
+ _last_update_usec = _last_gyro_timestamp;
+
+ _accel = _accel_sum / _accel_sum_count;
+ _accel_sum.zero();
+ _accel_sum_count = 0;
+
+ _gyro = _gyro_sum / _gyro_sum_count;
+ _gyro_sum.zero();
+ _gyro_sum_count = 0;
+
+ hal.scheduler->resume_timer_procs();
+
+ // add offsets and rotation
+ _accel.rotate(_board_orientation);
+ _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
+ _gyro -= _gyro_offset;
+
+#if 0
+// whats this all about????
+ if (_last_filter_hz != _mpu6000_filter) {
+ _set_filter_frequency(_mpu6000_filter);
+ _last_filter_hz = _mpu6000_filter;
+ }
+#endif
+ return true;
+}
+
+float AP_InertialSensor_Flymaple::get_delta_time(void)
+{
+ return _delta_time;
+}
+
+uint32_t AP_InertialSensor_Flymaple::get_last_sample_time_micros(void)
+{
+ return _last_update_usec;
+}
+
+float AP_InertialSensor_Flymaple::get_gyro_drift_rate(void)
+{
+ // 0.5 degrees/second/minute
+ return ToRad(0.5/60);
+}
+
+void AP_InertialSensor_Flymaple::_accumulate(void)
+{
+ if (_in_accumulate) {
+ return;
+ }
+ _in_accumulate = true;
+
+ // Read accelerometer
+ uint8_t buffer[8];
+ if (hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 8, buffer) == 0)
+ {
+ int16_t x = -((((int16_t)buffer[1]) << 8) | buffer[0]); // X axis
+ int16_t y = (((int16_t)buffer[3]) << 8) | buffer[2]; // Y axis
+ int16_t z = -((((int16_t)buffer[5]) << 8) | buffer[4]); // Z axis
+ _accel_sum += Vector3f(x, y, z);
+ _accel_sum_count++;
+ _last_accel_timestamp = hal.scheduler->micros();
+ }
+
+ // Read gyro
+ if (hal.i2c->readRegisters(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_GYROX_H, 6, buffer) == 0)
+ {
+ int16_t x = (((int16_t)buffer[0]) << 8) | buffer[1]; // X axis
+ int16_t y = (((int16_t)buffer[2]) << 8) | buffer[3]; // Y axis
+ int16_t z = -((((int16_t)buffer[4]) << 8) | buffer[5]); // Z axis
+ _gyro_sum += Vector3f(x, y, z);
+ _gyro_sum_count++;
+ _last_gyro_timestamp = hal.scheduler->micros();
+ }
+
+ _in_accumulate = false;
+}
+
+void AP_InertialSensor_Flymaple::_ins_timer(uint32_t now)
+{
+ _accumulate();
+}
+
+uint16_t AP_InertialSensor_Flymaple::num_samples_available(void)
+{
+ _accumulate();
+ return min(_accel_sum_count, _gyro_sum_count) / _sample_divider;
+}
+
+#endif // CONFIG_HAL_BOARD
+
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
new file mode 100644
index 0000000000..c09c6f5dca
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
@@ -0,0 +1,51 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_INERTIAL_SENSOR_FLYMAPLE_H__
+#define __AP_INERTIAL_SENSOR_FLYMAPLE_H__
+
+#include
+#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
+
+#include
+#include "AP_InertialSensor.h"
+
+class AP_InertialSensor_Flymaple : public AP_InertialSensor
+{
+public:
+
+ AP_InertialSensor_Flymaple() : AP_InertialSensor() {}
+
+ /* Concrete implementation of AP_InertialSensor functions: */
+ bool update();
+ float get_delta_time();
+ uint32_t get_last_sample_time_micros();
+ float get_gyro_drift_rate();
+ uint16_t num_samples_available();
+
+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;
+ static Vector3f _accel_sum;
+ static uint32_t _accel_sum_count;
+ static Vector3f _gyro_sum;
+ static uint32_t _gyro_sum_count;
+ static volatile bool _in_accumulate;
+ static uint64_t _last_accel_timestamp;
+ static uint64_t _last_gyro_timestamp;
+ uint8_t _sample_divider;
+
+ // support for updating filter at runtime
+ uint8_t _last_filter_hz;
+ uint8_t _default_filter_hz;
+
+ void _set_filter_frequency(uint8_t filter_hz);
+
+ // accelerometer and gyro driver handles
+ static int _accel_fd;
+ static int _gyro_fd;
+};
+#endif
+#endif // __AP_INERTIAL_SENSOR_FLYMAPLE_H__