Создание собственной системы стабилизации
урааааа у меня зажегся первый светодиод(проверочный) !!! 😃
пишем исключительно полусловами (16 бит), вот моя перекладка демки
я имел ввиду адрес сектора отрезанного под еепромку 😃
ну с одной мёртвой точки сдвинулся, пойду дальше копать…
урааааа у меня зажегся первый светодиод(проверочный) !!!
Поздравляю! С флешей разобрался? Вообщем от Ф3 практически не отличается, кроме возможности установить ширину данных, и ещё не увидел в описании процедуры программирования снятие бита программирования
PG bit in the FLASH_CR register, надо смотреть стандартные библиотеки
я имел ввиду адрес сектора отрезанного под еепромку
ну тут без вариантов uint32_t
так у меня проблема - проц запускается на частоте - 53.76 Мгц - чё я где утварил?
Виноват, сам скопировал от Discovery system_stm32f4xx.c и забыл про это…
и ещё не увидел в описании процедуры программирования снятие бита программирования
PG bit in the FLASH_CR register,
тут?
stm32f4xx_flash.c
if(status == FLASH_COMPLETE)
{
/* if the previous operation is completed, proceed to program the new data */
FLASH->CR &= CR_PSIZE_MASK;
FLASH->CR |= FLASH_PSIZE_BYTE;
FLASH->CR |= FLASH_CR_PG;
*(__IO uint8_t*)Address = Data;
/* Wait for last operation to be completed */
status = FLASH_WaitForLastOperation();
/* if the program operation is completed, disable the PG Bit */
FLASH->CR &= (~FLASH_CR_PG);
}
У меня всё отлично заработало, писал в старший сектор, чтоб линкёр в него не лез уменьшил верхнюю границу флеши в настройке проекта.
посмотри st-link видит эту область, а то когда я по незнанке сносил нулевой сектор, то видел что он стирается и в него пишется, а так нет, а отпаивать, что-бы выдрать ввесь дамп, ну не хочется - при запаяном программаторе через uart не достучишься… ага вспомнил, если бы с виртуальником были бы траблы, он бы мне сообщил - светодиод бы моргал часто-часто, да и дальше бы не пошел…
сейчас дошел до автодетекта (его выкинуть надо), а так как нечего детектировать то висим перемигиваемся светодиодиками - почему и заметил, что как-то не так моргают, полез частоту проверять…
тут? stm32f4xx_flash.c
Да, ту всё нормально, а в доке на Ф3 нет ничего про снятие бита программирования.
посмотри st-link видит эту область,
СТ-ЛИНК видит-читает, но непосредственно править в отладчике не даёт, только через программирование.
да-да у меня тоже всё прекрасно записано - отладчиком сейчас заглянул в седьмой сектор 0x08060000 - и до победы, занято только примерно пол килобайта, ещё 127,5 - свободны 😃
#include "board.h"
#include "mw.h"
#include <string.h>
#ifndef FLASH_PAGE_COUNT
#define FLASH_PAGE_COUNT 3
#endif
#define ADDR_FLASH_SECTOR_0 ((uint32_t)0x08000000) /* Base @ of Sector 0, 16 Kbytes */
#define ADDR_FLASH_SECTOR_1 ((uint32_t)0x08004000) /* Base @ of Sector 1, 16 Kbytes */
#define ADDR_FLASH_SECTOR_2 ((uint32_t)0x08008000) /* Base @ of Sector 2, 16 Kbytes */
#define ADDR_FLASH_SECTOR_3 ((uint32_t)0x0800C000) /* Base @ of Sector 3, 16 Kbytes */
#define ADDR_FLASH_SECTOR_4 ((uint32_t)0x08010000) /* Base @ of Sector 4, 64 Kbytes */
#define ADDR_FLASH_SECTOR_5 ((uint32_t)0x08020000) /* Base @ of Sector 5, 128 Kbytes */
#define ADDR_FLASH_SECTOR_6 ((uint32_t)0x08040000) /* Base @ of Sector 6, 128 Kbytes */
#define ADDR_FLASH_SECTOR_7 ((uint32_t)0x08060000) /* Base @ of Sector 7, 128 Kbytes */
#define FLASH_PAGE_SIZE ((uint32_t)0x20000)
#define FLASH_WRITE_ADDR ADDR_FLASH_SECTOR_7 //(0x08000000 + (uint32_t)FLASH_PAGE_SIZE*10 * (FLASH_PAGE_COUNT - 1)) // use the last KB for storage
config_t cfg;
const char rcChannelLetters[] = "AERT1234";
static uint8_t EEPROM_CONF_VERSION = 40;
static uint32_t enabledSensors = 0;
static void resetConf(void);
void parseRcChannels(const char *input)
{
const char *c, *s;
for (c = input; *c; c++) {
s = strchr(rcChannelLetters, *c);
if (s)
cfg.rcmap[s - rcChannelLetters] = c - input;
}
}
static uint8_t validEEPROM(void)
{
const config_t *temp = (const config_t *)FLASH_WRITE_ADDR;//;ADDR_FLASH_SECTOR_7
const uint8_t *p;
uint8_t chk = 0;
// check version number
if (EEPROM_CONF_VERSION != temp->version)
return 0;
// check size and magic numbers
if (temp->size != sizeof(config_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
return 0;
// verify integrity of temporary copy
for (p = (const uint8_t *)temp; p < ((const uint8_t *)temp + sizeof(config_t)); p++)
chk ^= *p;
// checksum failed
if (chk != 0)
return 0;
// looks good, let's roll!
return 1;
}
void readEEPROM(void)
{
uint8_t i;
// Read flash
memcpy(&cfg, (char *)FLASH_WRITE_ADDR, sizeof(config_t));//ADDR_FLASH_SECTOR_7
for (i = 0; i < 6; i++)
lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500;
for (i = 0; i < 11; i++) {
int16_t tmp = 10 * i - cfg.thrMid8;
uint8_t y = 1;
if (tmp > 0)
y = 100 - cfg.thrMid8;
if (tmp < 0)
y = cfg.thrMid8;
lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t) cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10; // [0;1000]
lookupThrottleRC[i] = cfg.minthrottle + (int32_t) (cfg.maxthrottle - cfg.minthrottle) * lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
}
cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
}
void writeParams(uint8_t b)
{
FLASH_Status status;
uint32_t i;
uint8_t chk = 0;
const uint8_t *p;
cfg.version = EEPROM_CONF_VERSION;
cfg.size = sizeof(config_t);
cfg.magic_be = 0xBE;
cfg.magic_ef = 0xEF;
cfg.chk = 0;
// recalculate checksum before writing
for (p = (const uint8_t *)&cfg; p < ((const uint8_t *)&cfg + sizeof(config_t)); p++)
chk ^= *p;
cfg.chk = chk;
// write it
FLASH_Unlock();
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR|FLASH_FLAG_PGSERR);
if (FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3) == FLASH_COMPLETE) {
for (i = 0; i < sizeof(config_t); i += 4) {
status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *) ((char *) &cfg + i));//ADDR_FLASH_SECTOR_7
if (status != FLASH_COMPLETE)
break; // TODO: fail
}
}
FLASH_Lock();
readEEPROM();
if (b)
blinkLED(15, 20, 1);
}
void checkFirstTime(bool reset)
{
// check the EEPROM integrity before resetting values
if (!validEEPROM() || reset)
resetConf();
}
// Default settings
static void resetConf(void)
{
int i;
const int8_t default_align[3][3] = { /* GYRO */ { 0, 0, 0 }, /* ACC */ { 0, 0, 0 }, /* MAG */ { -2, -3, 1 } };
memset(&cfg, 0, sizeof(config_t));
cfg.version = EEPROM_CONF_VERSION;
cfg.mixerConfiguration = MULTITYPE_QUADX;
featureClearAll();
setFeature(FEATURE_VBAT);
// cfg.looptime = 0;
cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30;
cfg.D8[ROLL] = 23;
cfg.P8[PITCH] = 40;
cfg.I8[PITCH] = 30;
cfg.D8[PITCH] = 23;
cfg.P8[YAW] = 55;
cfg.I8[YAW] = 45;
// cfg.D8[YAW] = 0;
cfg.P8[PIDALT] = 11;
cfg.I8[PIDALT] = 15;
cfg.D8[PIDALT] = 7;
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
// cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
// cfg.D8[PIDPOS] = 0;
cfg.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10;
cfg.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100;
cfg.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000;
cfg.P8[PIDNAVR] = 14; // NAV_P * 10;
cfg.I8[PIDNAVR] = 20; // NAV_I * 100;
cfg.D8[PIDNAVR] = 80; // NAV_D * 1000;
cfg.P8[PIDLEVEL] = 70;
cfg.I8[PIDLEVEL] = 10;
cfg.D8[PIDLEVEL] = 20;
cfg.P8[PIDMAG] = 40;
// cfg.P8[PIDVEL] = 0;
// cfg.I8[PIDVEL] = 0;
// cfg.D8[PIDVEL] = 0;
cfg.rcRate8 = 99;
cfg.rcExpo8 = 20;
// cfg.rollPitchRate = 0;
// cfg.yawRate = 0;
// cfg.dynThrPID = 0;
cfg.thrMid8 = 50;
// cfg.thrExpo8 = 0;
// for (i = 0; i < CHECKBOXITEMS; i++)
// cfg.activate[i] = 0;
// cfg.angleTrim[0] = 0;
// cfg.angleTrim[1] = 0;
// cfg.accZero[0] = 0;
// cfg.accZero[1] = 0;
// cfg.accZero[2] = 0;
// cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
memcpy(&cfg.align, default_align, sizeof(cfg.align));
cfg.acc_hardware = ACC_DEFAULT; // default/autodetect
cfg.acc_lpf_factor = 4;
cfg.acc_lpf_for_velocity = 10;
cfg.accz_deadband = 50;
cfg.gyro_cmpf_factor = 400; // default MWC
cfg.gyro_lpf = 42;
cfg.mpu6050_scale = 1; // fuck invensense
cfg.baro_tab_size = 21;
cfg.baro_noise_lpf = 0.6f;
cfg.baro_cf = 0.985f;
cfg.moron_threshold = 32;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.vbatscale = 110;
cfg.vbatmaxcellvoltage = 43;
cfg.vbatmincellvoltage = 33;
// cfg.power_adc_channel = 0;
// Radio
parseRcChannels("AETR1234");
// cfg.deadband = 0;
// cfg.yawdeadband = 0;
cfg.alt_hold_throttle_neutral = 20;
// cfg.spektrum_hires = 0;
cfg.midrc = 1500;
cfg.mincheck = 1100;
cfg.maxcheck = 1900;
// cfg.retarded_arm = 0; // disable arm/disarm on roll left/right
// Failsafe Variables
cfg.failsafe_delay = 10; // 1sec
cfg.failsafe_off_delay = 200; // 20sec
cfg.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
// Motor/ESC/Servo
cfg.minthrottle = 1150;
cfg.maxthrottle = 1850;
cfg.mincommand = 1000;
cfg.motor_pwm_rate = 490;
cfg.servo_pwm_rate = 50;
// servos
cfg.yaw_direction = 1;
cfg.tri_yaw_middle = 1500;
cfg.tri_yaw_min = 1020;
cfg.tri_yaw_max = 2000;
// fixed wing
cfg.pitch_min = 1020;
cfg.pitch_mid = 1500;
cfg.pitch_max = 1980;
cfg.yaw_min = 1020;
cfg.yaw_mid = 1500;
cfg.yaw_max = 1980;
cfg.flaperons = 0; // enable (1) / disable (0) flaperon function
cfg.flap_aux = 0; // which AUX channel use to toggle flaps
cfg.vtail = 0; // airplane tail configuration, X (elev + rudd) [0] or V tail [1]
cfg.pitch_direction = 1; // airplane pitch servo orientation
cfg.pitch_direction_l = 1; // (Flying-wing only) left servo - pitch orientation
cfg.pitch_direction_r = -1; // (Flying-wing only) right servo - pitch orientation (opposite sign to pitch_direction_l if servos are mounted mirrored)
cfg.roll_direction_l = 1; // left servo - roll orientation
cfg.roll_direction_r = -1; // right servo - roll orientation (same sign as ROLL_DIRECTION_L, if servos are mounted in mirrored orientation)
// flying wing
cfg.wing_left_min = 1020;
cfg.wing_left_mid = 1500;
cfg.wing_left_max = 1980;
cfg.wing_right_min = 1020;
cfg.wing_right_mid = 1500;
cfg.wing_right_max = 1980;
cfg.pitch_direction_l = 1;
cfg.pitch_direction_r = -1;
cfg.roll_direction_l = 1;
cfg.roll_direction_r = 1;
// gimbal
cfg.gimbal_pitch_gain = 10;
cfg.gimbal_roll_gain = 10;
cfg.gimbal_flags = GIMBAL_NORMAL;
cfg.gimbal_pitch_min = 1020;
cfg.gimbal_pitch_max = 2000;
cfg.gimbal_pitch_mid = 1500;
cfg.gimbal_roll_min = 1020;
cfg.gimbal_roll_max = 2000;
cfg.gimbal_roll_mid = 1500;
// gps/nav stuff
cfg.gps_type = GPS_NMEA;
cfg.gps_baudrate = 115200;
cfg.gps_wp_radius = 200;
cfg.gps_lpf = 20;
cfg.nav_slew_rate = 30;
cfg.nav_controls_heading = 1;
cfg.nav_speed_min = 100;
cfg.nav_speed_max = 300;
// serial (USART1) baudrate
cfg.serial_baudrate = 115200;
// custom mixer. clear by defaults.
for (i = 0; i < MAX_MOTORS; i++)
cfg.customMixer[i].throttle = 0.0f;
writeParams(0);
}
bool getSensors(uint32_t mask)
{
return enabledSensors & mask;
}
void setSensors(uint32_t mask)
{
enabledSensors |= mask;
}
void clearSensors(uint32_t mask)
{
enabledSensors &= ~(mask);
}
uint32_t sensorsMask(void)
{
return enabledSensors;
}
bool getFeature(uint32_t mask)
{
return cfg.enabledFeatures & mask;
}
void setFeature(uint32_t mask)
{
cfg.enabledFeatures |= mask;
}
void featureClear(uint32_t mask)
{
cfg.enabledFeatures &= ~(mask);
}
void featureClearAll()
{
cfg.enabledFeatures = 0;
}
uint32_t featureMask(void)
{
return cfg.enabledFeatures;
}
занято только примерно пол килобайта, ещё 127,5 - свободны
Жалко тереть такую страницу, лучше всёж попытаться уйти в мелкий сектор.
ну пока не напрягает, у меня всего кода на 68к… хотя можно смело спрыгнуть в первый сектор (не нулевой)…
Сергей, у тебя к этому порту ГУЙ Вийский или что-то своё?
Вий но пока до него не добрался… в вингуи сделали сразу терминал, разберусь с датчиками буду пробовать цепляться…
тут обновил
Запускаем .NET MicroFramework на STM32F4Discovery
ток в комментах пишут, что перформанс совсем дохленький…
Добрался до гиро-акселя, пока присматриваю откуда спилить, вопрос такого плана DMA на SPI1 (там мпу висит) использовать?
вопрос такого плана DMA на SPI1 (там мпу висит) использовать?
Конечно использовать, это пзволит избежать затрат времени на тупую пересылку байт. Вообще можно по готовности ДУСа читтать весь набор регистров в буфер, а потом по необходимости программно разбирать.
Буду очень благодарен, если кто нибудь доходчиво пояснит ЧТО и КАК фильтрует HIGH PASS Filter в датчиках, его практическое применение, в переводе с даташита так и не понял…
получается как-то, не слишком хорошо, DMA2: Stream 0, Channel 0 - ADC1; DMA2: Stream 2, Channel 4 - USART1_RX; Stream 7, Channel 4 - USART1_TX;
ага разобрался сделать: DMA2: Stream 5, Channel 4 - USART1_RX; Stream 7, Channel 4 - USART1_TX; и Stream 2, Channel 3 - SPI1_RX; Stream 3 Channel 3 - SPI1_TX;
картинко
ЧТО и КАК фильтрует HIGH PASS Filter
высокочастотный фильтр, пропускающий частоты выше некоторой частоты среза и ослабляющий более низкие частоты…
ну типа решили мы полетать в дождь, а у нас две-три капли в секунду барабанят (ну в моём случае по салатнице) - аксель с ума сводят - можно обрезать данное мероприятие (к гироскопам не применимо)…
о только щас подумал, а нафига акселю вообще вибрации чувствовать - зарезать с двух сторон - пусть доплывает потиху?
Stream 3 Channel 3 - SPI1_TX;
а это наверно и ненадо, зачем мне буфер на выход если я один раз при инициализации записываю данные?
это наверно и ненадо, зачем мне буфер на выход если я один раз при инициализации записываю данные?
Не факт, а адрес в начале запроса данных передавать не нужно?
Вот вроде удобоворимо доделал прошивку для Ф3, может блином летать, все эксперементальные функции отключены. Настраивается через терминал в кодировке вин1251, клавишами со стрелками и ввода, там вроде всё понятно.
Не факт, а адрес в начале запроса данных передавать не нужно?
ну да чёт я как-то не подумал…
Наверно без помощи я тут застряну, передача - тут можно и последовательно регистры программировать - засылать адрес регистра плюс по 8 бит в регистр, а вот приём - надо сразу хапнуть из 14 регистров (6-гиро, 6-аксель, 2-температура) да ещё и разложить их по полочкам (в dma), голова кругом, да ещё задумал два отдельных драйвера: spi и mpu, с i2c хоть худо бедно разобрался, а тут ещё начитался всякого, даже пока не знаю за что хвататься…
Пока честно сколхоженый драйвер MPU:
//drv_mpu6000
#include "board.h"
// MPU_INT on PB0
// Experimental DMP support
// #define MPU6000_DMP
#define DMP_MEM_START_ADDR 0x6E
#define DMP_MEM_R_W 0x6F
#define INV_MAX_NUM_ACCEL_SAMPLES (8)
#define DMP_REF_QUATERNION (0)
#define DMP_REF_GYROS (DMP_REF_QUATERNION + 4) // 4
#define DMP_REF_CONTROL (DMP_REF_GYROS + 3) // 7
#define DMP_REF_RAW (DMP_REF_CONTROL + 4) // 11
#define DMP_REF_RAW_EXTERNAL (DMP_REF_RAW + 8) // 19
#define DMP_REF_ACCEL (DMP_REF_RAW_EXTERNAL + 6) // 25
#define DMP_REF_QUANT_ACCEL (DMP_REF_ACCEL + 3) // 28
#define DMP_REF_QUATERNION_6AXIS (DMP_REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES) // 36
#define DMP_REF_EIS (DMP_REF_QUATERNION_6AXIS + 4) // 40
#define DMP_REF_DMP_PACKET (DMP_REF_EIS + 3) // 43
#define DMP_REF_GARBAGE (DMP_REF_DMP_PACKET + 1) // 44
#define DMP_REF_LAST (DMP_REF_GARBAGE + 1) // 45
#define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
#define MPU_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
#define MPU_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
#define MPU_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
#define MPU_RA_XA_OFFS_L_TC 0x07
#define MPU_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
#define MPU_RA_YA_OFFS_L_TC 0x09
#define MPU_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
#define MPU_RA_ZA_OFFS_L_TC 0x0B
#define MPU_RA_PRODUCT_ID 0x0C // Product ID Register
#define MPU_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
#define MPU_RA_XG_OFFS_USRL 0x14
#define MPU_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
#define MPU_RA_YG_OFFS_USRL 0x16
#define MPU_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
#define MPU_RA_ZG_OFFS_USRL 0x18
#define MPU_RA_SMPLRT_DIV 0x19
#define MPU_RA_CONFIG 0x1A
#define MPU_RA_GYRO_CONFIG 0x1B
#define MPU_RA_ACCEL_CONFIG 0x1C
#define MPU_RA_FF_THR 0x1D
#define MPU_RA_FF_DUR 0x1E
#define MPU_RA_MOT_THR 0x1F
#define MPU_RA_MOT_DUR 0x20
#define MPU_RA_ZRMOT_THR 0x21
#define MPU_RA_ZRMOT_DUR 0x22
#define MPU_RA_FIFO_EN 0x23
#define MPU_RA_I2C_MST_CTRL 0x24
#define MPU_RA_I2C_SLV0_ADDR 0x25
#define MPU_RA_I2C_SLV0_REG 0x26
#define MPU_RA_I2C_SLV0_CTRL 0x27
#define MPU_RA_I2C_SLV1_ADDR 0x28
#define MPU_RA_I2C_SLV1_REG 0x29
#define MPU_RA_I2C_SLV1_CTRL 0x2A
#define MPU_RA_I2C_SLV2_ADDR 0x2B
#define MPU_RA_I2C_SLV2_REG 0x2C
#define MPU_RA_I2C_SLV2_CTRL 0x2D
#define MPU_RA_I2C_SLV3_ADDR 0x2E
#define MPU_RA_I2C_SLV3_REG 0x2F
#define MPU_RA_I2C_SLV3_CTRL 0x30
#define MPU_RA_I2C_SLV4_ADDR 0x31
#define MPU_RA_I2C_SLV4_REG 0x32
#define MPU_RA_I2C_SLV4_DO 0x33
#define MPU_RA_I2C_SLV4_CTRL 0x34
#define MPU_RA_I2C_SLV4_DI 0x35
#define MPU_RA_I2C_MST_STATUS 0x36
#define MPU_RA_INT_PIN_CFG 0x37
#define MPU_RA_INT_ENABLE 0x38
#define MPU_RA_DMP_INT_STATUS 0x39
#define MPU_RA_INT_STATUS 0x3A
#define MPU_RA_ACCEL_XOUT_H 0x3B
#define MPU_RA_ACCEL_XOUT_L 0x3C
#define MPU_RA_ACCEL_YOUT_H 0x3D
#define MPU_RA_ACCEL_YOUT_L 0x3E
#define MPU_RA_ACCEL_ZOUT_H 0x3F
#define MPU_RA_ACCEL_ZOUT_L 0x40
#define MPU_RA_TEMP_OUT_H 0x41
#define MPU_RA_TEMP_OUT_L 0x42
#define MPU_RA_GYRO_XOUT_H 0x43
#define MPU_RA_GYRO_XOUT_L 0x44
#define MPU_RA_GYRO_YOUT_H 0x45
#define MPU_RA_GYRO_YOUT_L 0x46
#define MPU_RA_GYRO_ZOUT_H 0x47
#define MPU_RA_GYRO_ZOUT_L 0x48
#define MPU_RA_EXT_SENS_DATA_00 0x49
#define MPU_RA_MOT_DETECT_STATUS 0x61
#define MPU_RA_I2C_SLV0_DO 0x63
#define MPU_RA_I2C_SLV1_DO 0x64
#define MPU_RA_I2C_SLV2_DO 0x65
#define MPU_RA_I2C_SLV3_DO 0x66
#define MPU_RA_I2C_MST_DELAY_CTRL 0x67
#define MPU_RA_SIGNAL_PATH_RESET 0x68
#define MPU_RA_MOT_DETECT_CTRL 0x69
#define MPU_RA_USER_CTRL 0x6A
#define MPU_RA_PWR_MGMT_1 0x6B
#define MPU_RA_PWR_MGMT_2 0x6C
#define MPU_RA_BANK_SEL 0x6D
#define MPU_RA_MEM_START_ADDR 0x6E
#define MPU_RA_MEM_R_W 0x6F
#define MPU_RA_DMP_CFG_1 0x70
#define MPU_RA_DMP_CFG_2 0x71
#define MPU_RA_FIFO_COUNTH 0x72
#define MPU_RA_FIFO_COUNTL 0x73
#define MPU_RA_FIFO_R_W 0x74
#define MPU_RA_WHO_AM_I 0x75
#define MPU6000_SMPLRT_DIV 0 //8000Hz
#define MPU6000_DLPF_CFG 0 // 256Hz
//#define MPU6000_DLPF_CFG 1 // 188Hz
//#define MPU6000_DLPF_CFG 2 // 98Hz
//#define MPU6000_DLPF_CFG 3 // 42Hz
#define MPU6000ES_REV_C4 0x14
#define MPU6000ES_REV_C5 0x15
#define MPU6000ES_REV_D6 0x16
#define MPU6000ES_REV_D7 0x17
#define MPU6000ES_REV_D8 0x18
#define MPU6000_REV_C4 0x54
#define MPU6000_REV_C5 0x55
#define MPU6000_REV_D6 0x56
#define MPU6000_REV_D7 0x57
#define MPU6000_REV_D8 0x58
#define MPU6000_REV_D9 0x59
static void mpu6000AccInit(void);
static void mpu6000AccRead(int16_t * accData);
static void mpu6000AccAlign(int16_t * accData);
static void mpu6000GyroInit(void);
static void mpu6000GyroRead(int16_t * gyroData);
static void mpu6000GyroAlign(int16_t * gyroData);
#ifdef MPU6000_DMP
static void mpu6000DmpInit(void);
float dmpdata[2];
int16_t dmpGyroData[3];
#endif
extern uint16_t acc_1G;
static uint8_t mpuAccelHalf = 0;
bool mpu6000Detect(sensor_t * acc, sensor_t * gyro, uint8_t *scale)
{
bool ack;
uint8_t sig, rev;
uint8_t tmp[6];
delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe
ack = SPI1_Read( MPU_RA_WHO_AM_I, 1, &sig);
if (!ack)
return false;
// So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
// The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0’s 7-bit I2C address.
// The least significant bit of the MPU-60X0’s I2C address is determined by the value of the AD0 pin. (we know that already).
// But here's the best part: The value of the AD0 pin is not reflected in this register.
if (sig != (0x7e))
return false;
// determine product ID and accel revision
SPI1_Read(MPU_RA_XA_OFFS_H, 6, tmp);
rev = ((tmp[5] & 0x01) << 2) | ((tmp[3] & 0x01) << 1) | (tmp[1] & 0x01);
if (rev) {
/* Congrats, these parts are better. */
if (rev == 1) {
mpuAccelHalf = 1;
} else if (rev == 2) {
mpuAccelHalf = 0;
} else {
failureMode(5);
}
} else {
SPI_Read( MPU_RA_PRODUCT_ID, 1, &sig);
rev = sig & 0x0F;
if (!rev) {
failureMode(5);
} else if (rev == 4) {
mpuAccelHalf = 1;
} else {
mpuAccelHalf = 0;
}
}
acc->init = mpu6000AccInit;
acc->read = mpu6000AccRead;
acc->align = mpu6000AccAlign;
gyro->init = mpu6000GyroInit;
gyro->read = mpu6000GyroRead;
gyro->align = mpu6000GyroAlign;
// give halfacc (old revision) back to system
if (scale)
*scale = mpuAccelHalf;
#ifdef MPU6000_DMP
mpu6000DmpInit();
#endif
return true;
}
static void mpu6000AccInit(void)
{
if (mpuAccelHalf)
acc_1G = 255;
else
acc_1G = 512;
}
static void mpu6000AccRead(int16_t *accData)
{
uint8_t buf[6];
#ifndef MPU6000_DMP
SPI1_Read( MPU_RA_ACCEL_XOUT_H, 6, buf);
accData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 8;
accData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 8;
accData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 8;
#else
accData[0] = accData[1] = accData[2] = 0;
#endif
}
static void mpu6000AccAlign(int16_t *accData)
{
int16_t temp[2];
temp[0] = accData[0];
temp[1] = accData[1];
// official direction is RPY
accData[0] = temp[1];
accData[1] = -temp[0];
accData[2] = accData[2];
}
static void mpu6000GyroInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
// PB13 - MPU_INT output on rev4 hardware
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_Init(GPIOB, &GPIO_InitStructure);
#ifndef MPU6000_DMP
SPI1_Write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(5);
SPI1_Write( MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
SPI1_Write( MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
SPI1_Write( MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
SPI1_Write( MPU_RA_CONFIG, MPU6000_DLPF_CFG); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
SPI1_Write( MPU_RA_GYRO_CONFIG, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
// Accel scale 8g (4096 LSB/g)
SPI1_Write( MPU_RA_ACCEL_CONFIG, 2 << 3);
#endif
}
static void mpu6000GyroRead(int16_t * gyroData)
{
uint8_t buf[6];
#ifndef MPU6000_DMP
SPI1_Read(MPU_RA_GYRO_XOUT_H, 6, buf);
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
#else
gyroData[0] = dmpGyroData[0] / 4 ;
gyroData[1] = dmpGyroData[1] / 4;
gyroData[2] = dmpGyroData[2] / 4;
#endif
}
static void mpu6000GyroAlign(int16_t * gyroData)
{
// official direction is RPY
gyroData[0] = gyroData[0];
gyroData[1] = gyroData[1];
gyroData[2] = -gyroData[2];
}
#ifdef MPU6000_DMP
//This 3D array contains the default DMP memory bank binary that gets loaded during initialization.
//In the Invensense UC3-A3 firmware this is uploaded in 128 byte tranmissions, but the Arduino Wire
//library only supports 32 byte transmissions, including the register address to which you're writing,
//so I broke it up into 16 byte transmission payloads which are sent in the dmp_init() function below.
//
//This was reconstructed from observed I2C traffic generated by the UC3-A3 demo code, and not extracted
//directly from that code. That is true of all transmissions in this sketch, and any documentation has
//been added after the fact by referencing the Invensense code.
const unsigned char dmpMem[8][16][16] = {
{
{0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00},
{0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01},
{0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01},
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00},
{0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00},
{0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82},
{0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC},
{0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4},
{0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10}
},
{
{0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00},
{0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8},
{0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C},
{0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C},
{0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0}
},
{
{0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00},
{0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
{0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
},
{
{0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F},
{0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2},
{0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF},
{0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C},
{0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1},
{0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01},
{0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80},
{0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C},
{0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80},
{0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E},
{0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9},
{0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24},
{0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0},
{0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86},
{0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1},
{0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86}
},
{
{0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA},
{0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C},
{0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8},
{0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3},
{0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84},
{0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5},
{0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3},
{0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1},
{0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5},
{0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D},
{0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9},
{0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D},
{0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9},
{0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A},
{0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8},
{0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87}
},
{
{0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8},
{0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68},
{0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D},
{0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94},
{0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA},
{0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56},
{0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9},
{0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA},
{0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A},
{0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60},
{0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97},
{0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04},
{0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78},
{0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79},
{0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68},
{0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68}
},
{
{0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04},
{0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66},
{0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31},
{0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60},
{0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76},
{0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56},
{0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD},
{0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91},
{0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8},
{0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE},
{0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9},
{0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD},
{0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E},
{0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8},
{0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89},
{0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79}
},
{
{0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8},
{0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA},
{0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB},
{0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3},
{0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3},
{0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3},
{0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3},
{0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC},
{0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF}
}
};
//DMP update transmissions (Bank, Start Address, Update Length, Update Data...)
const uint8_t dmp_updates[29][9] = {
{0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C}, //FCFG_1 inv_set_gyro_calibration
{0x03, 0xAB, 0x03, 0x36, 0x56, 0x76}, //FCFG_3 inv_set_gyro_calibration
{0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2}, //D_0_104 inv_set_gyro_calibration
{0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1}, //D_0_24 inv_set_gyro_calibration
{0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00}, //D_1_152 inv_set_accel_calibration
{0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97}, //FCFG_2 inv_set_accel_calibration
{0x03, 0x89, 0x03, 0x26, 0x46, 0x66}, //FCFG_7 inv_set_accel_calibration
{0x00, 0x6C, 0x02, 0x20, 0x00}, //D_0_108 inv_set_accel_calibration
{0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_00 inv_set_compass_calibration
{0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_01
{0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_02
{0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_10
{0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_11
{0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_12
{0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_20
{0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_21
{0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_22
{0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00}, //D_1_236 inv_apply_endian_accel
{0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97}, //FCFG_2 inv_set_mpu_sensors
{0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D}, //CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
{0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D}, //FCFG_5 inv_set_bias_update
{0x00, 0xA3, 0x01, 0x00}, //D_0_163 inv_set_dead_zone
//SET INT_ENABLE at i=22
{0x07, 0x86, 0x01, 0xFE}, //CFG_6 inv_set_fifo_interupt
{0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38}, //CFG_8 inv_send_quaternion
{0x07, 0x7E, 0x01, 0x30}, //CFG_16 inv_set_footer
{0x07, 0x46, 0x01, 0x9A}, //CFG_GYRO_SOURCE inv_send_gyro
{0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_9 inv_send_gyro -> inv_construct3_fifo
{0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_12 inv_send_accel -> inv_construct3_fifo
{0x02, 0x16, 0x02, 0x00, 0x00}, //D_0_22 inv_set_fifo_rate
};
static long dmp_lastRead = 0;
static uint8_t dmp_processed_packet[8];
static uint8_t dmp_received_packet[50];
static uint8_t dmp_temp = 0;
uint8_t dmp_fifoCountL = 0;
static uint8_t dmp_fifoCountL2 = 0;
static uint8_t dmp_packetCount = 0x00;
static bool dmp_longPacket = false;
static bool dmp_firstPacket = true;
static void mpu6000DmpMemInit(void);
static void mpu6000DmpBankSelect(uint8_t bank);
static bool mpu6000DmpFifoReady(void);
static void mpu6000DmpGetPacket(void);
static void mpu6000DmpProcessQuat(void);
void mpu6000DmpResetFifo(void);
static void mpu6000DmpInit(void)
{
uint8_t temp = 0;
SPI1_Write( MPU_RA_PWR_MGMT_1, 0xC0); // device reset
SPI1_Write( MPU_RA_PWR_MGMT_2, 0x00);
delay(10);
SPI1_Write( MPU_RA_PWR_MGMT_1, 0x00);
SPI1_Write( MPU_RA_BANK_SEL, 0x70);
SPI1_Write( MPU_RA_MEM_START_ADDR, 0x06);
SPI1_Read( MPU_RA_MEM_R_W, 1, &temp);
SPI1_Write( MPU_RA_BANK_SEL, 0x00);
SPI1_Write( MPU_RA_INT_PIN_CFG, 0x32); // I2C bypass enabled, latch int enabled, int read clear
SPI1_Read( MPU_RA_PWR_MGMT_1, 1, &temp);
delay(5);
mpu6000DmpMemInit();
}
void mpu6000DmpLoop(void)
{
uint8_t temp;
uint8_t buf[2];
if (mpu6000DmpFifoReady()) {
LED3_ON;
mpu6000DmpGetPacket();
SPI1_Read( MPU_RA_INT_STATUS, 1, &temp);
if (dmp_firstPacket) {
delay(1);
mpu6000DmpBankSelect(0x00);
mpu6000DmpBankSelect(0x00); // bank
SPI1_Write(MPU_RA_MEM_START_ADDR, 0x60);
SPI1_WriteBuffer( MPU_RA_MEM_R_W, 4, "\x04\x00\x00\x00"); // data
mpu6000DmpBankSelect(0x01);
SPI1_Write( MPU_RA_MEM_START_ADDR, 0x62);
SPI1_Read( DMP_MEM_R_W, 2, buf);
dmp_firstPacket = false;
mpu6000DmpFifoReady();
}
if (dmp_fifoCountL == 42) {
mpu6000DmpProcessQuat();
}
LED3_OFF;
}
}
#define dmp_quatTake32(a, b) (((a)[4*(b)+0]<<8) | ((a)[4*(b)+1]<<0))
extern int16_t angle[2];
static void mpu6000DmpProcessQuat(void)
{
float quat0, quat1, quat2, quat3;
int32_t quatl0, quatl1, quatl2, quatl3;
quatl0 = dmp_quatTake32(dmp_received_packet, 0);
quatl1 = dmp_quatTake32(dmp_received_packet, 1);
quatl2 = dmp_quatTake32(dmp_received_packet, 2);
quatl3 = dmp_quatTake32(dmp_received_packet, 3);
if (quatl0 > 32767)
quatl0 -= 65536;
if (quatl1 > 32767)
quatl1 -= 65536;
if (quatl2 > 32767)
quatl2 -= 65536;
if (quatl3 > 32767)
quatl3 -= 65536;
quat0 = ((float) quatl0) / 16384.0f;
quat1 = ((float) quatl1) / 16384.0f;
quat2 = ((float) quatl2) / 16384.0f;
quat3 = ((float) quatl3) / 16384.0f;
dmpdata[0] = atan2f(2 * ((quat0 * quat1) + (quat2 * quat3)), 1.0 - (2 * ((quat1 * quat1) + (quat2 * quat2)))) * (180.0f / M_PI);
dmpdata[1] = asinf(2 * ((quat0 * quat2) - (quat3 * quat1))) * (180.0f / M_PI);
angle[0] = dmpdata[0] * 10;
angle[1] = dmpdata[1] * 10;
dmpGyroData[0] = ((dmp_received_packet[4 * 4 + 0] << 8) | (dmp_received_packet[4 * 4 + 1] << 0));
dmpGyroData[1] = ((dmp_received_packet[4 * 5 + 0] << 8) | (dmp_received_packet[4 * 5 + 1] << 0));
dmpGyroData[2] = ((dmp_received_packet[4 * 6 + 0] << 8) | (dmp_received_packet[4 * 6 + 1] << 0));
}
void mpu6000DmpResetFifo(void)
{
uint8_t ctrl;
SPI1_Read( MPU_RA_USER_CTRL, 1, &ctrl);
ctrl |= 0x04;
SPI1_Write( MPU_RA_USER_CTRL, ctrl);
}
static void mpu6000DmpGetPacket(void)
{
if (dmp_fifoCountL > 32) {
dmp_fifoCountL2 = dmp_fifoCountL - 32;
dmp_longPacket = true;
}
if (dmp_longPacket) {
SPI1_Read( MPU_RA_FIFO_R_W, 32, dmp_received_packet);
SPI1_Read( MPU_RA_FIFO_R_W, dmp_fifoCountL, dmp_received_packet + 32);
dmp_longPacket = false;
} else {
SPI1_Read( MPU_RA_FIFO_R_W, dmp_fifoCountL, dmp_received_packet);
}
}
uint16_t dmpFifoLevel = 0;
static bool mpu6000DmpFifoReady(void)
{
uint8_t buf[2];
SPI1_Read( MPU_RA_FIFO_COUNTH, 2, buf);
dmp_fifoCountL = buf[1];
dmpFifoLevel = buf[0] << 8 | buf[1];
if (dmp_fifoCountL == 42 || dmp_fifoCountL == 44)
return true;
else {
// lame hack to empty out fifo, as dmpResetFifo doesn't actually seem to do it...
if (dmpFifoLevel > 100) {
// clear out fifo
uint8_t crap[16];
do {
SPI1_Read( MPU_RA_FIFO_R_W, dmpFifoLevel > 16 ? 16 : dmpFifoLevel, crap);
SPI1_Read( MPU_RA_FIFO_COUNTH, 2, buf);
dmpFifoLevel = buf[0] << 8 | buf[1];
} while (dmpFifoLevel);
}
}
return false;
}
static void mpu6000DmpBankSelect(uint8_t bank)
{
SPI1_Write( MPU_RA_BANK_SEL, bank);
}
static void mpu6000DmpBankInit(void)
{
uint8_t i, j;
uint8_t incoming[9];
for (i = 0; i < 7; i++) {
mpu6000DmpBankSelect(i);
for (j = 0; j < 16; j++) {
uint8_t start_addy = j * 0x10;
SPI1_Write( DMP_MEM_START_ADDR, start_addy);
SPI1_WriteBuffer( DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[i][j][0]);
}
}
mpu6000DmpBankSelect(7);
for (j = 0; j < 8; j++) {
uint8_t start_addy = j * 0x10;
SPI1_Write( DMP_MEM_START_ADDR, start_addy);
SPI1_WriteBuffer( DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[7][j][0]);
}
SPI1_Write( DMP_MEM_START_ADDR, 0x80);
SPI1_WriteBuffer( DMP_MEM_R_W, 9, (uint8_t *) & dmpMem[7][8][0]);
SPI1_Read( DMP_MEM_R_W, 8, incoming);
}
static void mpu6000DmpMemInit(void)
{
uint8_t i;
uint8_t temp;
mpu6000DmpBankInit();
// Bank, Start Address, Update Length, Update Data...
for (i = 0; i < 22; i++) {
mpu6000DmpBankSelect(dmp_updates[i][0]); // bank
SPI1_Write( DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
SPI1_WriteBuffer( DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
}
SPI1_Write( MPU_RA_INT_ENABLE, 0x32);
for (i = 22; i < 29; i++) {
mpu6000DmpBankSelect(dmp_updates[i][0]); // bank
SPI1_Write( DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
SPI1_WriteBuffer(DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
}
SPI1_Write(MPU_RA_INT_ENABLE, 0x02); // ??
SPI1_Write(MPU_RA_PWR_MGMT_1, 0x03); // CLKSEL = PLL w Z ref
SPI1_Write(MPU_RA_SMPLRT_DIV, 0x04);
SPI1_Write(MPU_RA_GYRO_CONFIG, 0x18); // full scale 2000 deg/s
SPI1_Write(MPU_RA_CONFIG, 0x0B); // ext_sync_set=temp_out_L, accel DLPF 44Hz, gyro DLPF 42Hz
SPI1_Write(MPU_RA_DMP_CFG_1, 0x03);
SPI1_Write(MPU_RA_DMP_CFG_2, 0x00);
SPI1_Write(MPU_RA_XG_OFFS_TC, 0x00);
SPI1_Write(MPU_RA_YG_OFFS_TC, 0x00);
SPI1_Write(MPU_RA_ZG_OFFS_TC, 0x00);
// clear offsets
SPI1_WriteBuffer( MPU_RA_XG_OFFS_USRH, 6, "\x00\x00\x00\x00\x00\x00"); // data
mpu6000DmpBankSelect(0x01); // bank
SPI1_Write( MPU_RA_MEM_START_ADDR, 0xB2);
SPI1_WriteBuffer( MPU_RA_MEM_R_W, 2, "\xFF\xFF"); // data
mpu6000DmpBankSelect(0x01); // bank
SPI1_Write( MPU_RA_MEM_START_ADDR, 0x90);
SPI1_WriteBuffer( MPU_RA_MEM_R_W, 4, "\x09\x23\xA1\x35"); // data
SPI1_Read( MPU_RA_USER_CTRL, 1, &temp);
SPI1_Write( MPU_RA_USER_CTRL, 0x04); // fifo reset
// Insert FIFO count read?
mpu6000DmpFifoReady();
SPI1_Write(MPU_RA_USER_CTRL, 0x00); // ?? I think this enables a lot of stuff but disables fifo
SPI1_Write(MPU_RA_PWR_MGMT_1, 0x03); // CLKSEL = PLL w Z ref
delay(2);
SPI1_Read( MPU_RA_PWR_MGMT_2, 1, &temp);
SPI1_Write( MPU_RA_PWR_MGMT_2, 0x00);
SPI1_Read( MPU_RA_ACCEL_CONFIG, 1, &temp);
SPI1_Write( MPU_RA_ACCEL_CONFIG, 0x00); // full scale range +/- 2g
delay(2);
SPI1_Read( MPU_RA_PWR_MGMT_1, 1, &temp);
SPI1_Write( MPU_RA_MOT_THR, 0x02);
SPI1_Write( MPU_RA_ZRMOT_THR, 0x9C);
SPI1_Write( MPU_RA_MOT_DUR, 0x50);
SPI1_Write( MPU_RA_ZRMOT_DUR, 0x00);
SPI1_Write( MPU_RA_USER_CTRL, 0x04); // fifo reset
SPI1_Write( MPU_RA_USER_CTRL, 0x00);
SPI1_Write( MPU_RA_USER_CTRL, 0xC8); // fifo enable
mpu6000DmpBankSelect(0x01); // bank
SPI1_Write( MPU_RA_MEM_START_ADDR, 0x6A);
SPI1_WriteBuffer( MPU_RA_MEM_R_W, 2, "\x06\x00"); // data
mpu6000DmpBankSelect(0x01); // bank
SPI1_Write( MPU_RA_MEM_START_ADDR, 0x60);
SPI1_WriteBuffer( MPU_RA_MEM_R_W, 8, "\x00\x00\x00\x00\x00\x00\x00\x00"); // data
mpu6000DmpBankSelect(0x00); // bank
SPI1_Write( MPU_RA_MEM_START_ADDR, 0x60);
SPI1_WriteBuffer( MPU_RA_MEM_R_W, 4, "\x40\x00\x00\x00"); // data
}
#else /* MPU6000_DMP */
void mpu6000DmpLoop(void)
{
}
void mpu6000DmpResetFifo(void)
{
}
#endif /* MPU6000_DMP */
ну и заголовочный к нему:
//drv_mpu6000.h
#pragma once
bool mpu6000Detect(sensor_t * acc, sensor_t * gyro, uint8_t *scale);
void mpu6000DmpLoop(void);
void mpu6000DmpResetFifo(void);
Сергей, SPI+DMA это вообще сказка, весь фокус в том что при записи и чтении ничего не меняется. Я делал так:
есть буфер в памяти размером со считываемый кусок данных+1 байт, в начало пишем слово управления, после чего заряжаем ПДП (только важно сначала разрешить чтение, а только потом запись иначе ПДП вытолкает начало буфера ничего при этом не прочитав), канала ПДП передатчика начинает выталкивать в SPI буфер, а канал чтения раскладывает на место переданных принятые, так до конца счётчиков ПДП. На окончание ПДП чтения заряжаешь прерывание, по этому прерванию разбираешь буфер. Как-то так…
uint8_t DUSbuffer[10];
/* настройка ПДП ДУС */
void DUS_DMA_Init(void)
{
DMA_InitTypeDef DMA_InitStruct;
EXTI_InitTypeDef EXTI_InitStructure;
/* Настройка ПДП для ДУС */
SPI_Cmd (L3GD20_SPI, ENABLE); /* разрешаем работу СПИ */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
/* заполняем структуру ПДП по умолчанию */
DMA_StructInit(&DMA_InitStruct);
DMA_InitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable; /* постоянный адрес перефирии */
DMA_InitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;/* размерность ус-ва - байт */
DMA_InitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable; /* счетчик буфера увеличивается */
DMA_InitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; /* размерность буфера - байт */
DMA_InitStruct.DMA_Mode = DMA_Mode_Normal; /* нормальный режим */
DMA_InitStruct.DMA_M2M = DMA_M2M_Disable; /* между перефирией и ОЗУ */
DMA_InitStruct.DMA_PeripheralBaseAddr = (uint32_t)&L3GD20_SPI->DR; /* зададем адрес приемника данных */
DMA_InitStruct.DMA_MemoryBaseAddr = (uint32_t)&DUSbuffer[0]; /* задаем адрес источника данных */
DMA_InitStruct.DMA_BufferSize = 9; /* указываем число пересылаемых данных */
/* применяем структуру для записи */
DMA_InitStruct.DMA_Priority = DMA_Priority_Medium; /* приоретет записи средний */
DMA_InitStruct.DMA_DIR = DMA_DIR_PeripheralDST; /* порт - приемник */
DMA_Init(DUS_DMA_Wr, &DMA_InitStruct);
/* применяем структуру для чтения */
DMA_InitStruct.DMA_Priority = DMA_Priority_High; /* приоретет чтения высокий */
DMA_InitStruct.DMA_DIR = DMA_DIR_PeripheralSRC; /* порт - источник */
DMA_Init(DUS_DMA_Rd, &DMA_InitStruct);
/* разрешить запрос ПДП для СПИ */
SPI_I2S_DMACmd(L3GD20_SPI, SPI_I2S_DMAReq_Rx, ENABLE);
SPI_I2S_DMACmd(L3GD20_SPI, SPI_I2S_DMAReq_Tx, ENABLE);
/* тактируем порт */
RCC_AHBPeriphClockCmd(L3GD20_SPI_INT2_GPIO_CLK, ENABLE);
/* конфигурация подключения к портам */
SYSCFG_EXTILineConfig(L3GD20_SPI_INT2_EXTI_PORT_SOURCE,L3GD20_SPI_INT2_EXTI_PIN_SOURCE);
/* запрос прерывания по готовности данных ДУС*/
EXTI_ClearITPendingBit(L3GD20_SPI_INT2_EXTI_LINE);
EXTI_InitStructure.EXTI_Line = L3GD20_SPI_INT2_EXTI_LINE;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
}
/* разрешаем работу петли ДУС */
void DUS_PetlyaInit(void)
{
/* перенастроим ПДП ДУСа*/
DUS_DMA_Init();
/* разрешаем прерывания от ПДП для канала чтения */
DMA_ITConfig(DUS_DMA_Rd, DMA_IT_TC, ENABLE);
/* разрешить запрос ПДП для СПИ */
SPI_I2S_DMACmd(L3GD20_SPI, SPI_I2S_DMAReq_Rx, ENABLE);
SPI_I2S_DMACmd(L3GD20_SPI, SPI_I2S_DMAReq_Tx, ENABLE);
/* разрешаем работу ПДП */
DMA_Cmd (DUS_DMA_Rd, ENABLE); /* сначала разрешаем чтение(!!!!) */
DMA_Cmd (DUS_DMA_Wr, ENABLE);
/* настройка прерывания после ПДП */
NVIC_EnableIRQ(DMA1_Channel2_IRQn);
NVIC_SetPriority(DMA1_Channel2_IRQn, 2);
/* настройка прерывания по линии готовности ДУС */
NVIC_EnableIRQ(L3GD20_SPI_INT2_EXTI_IRQn);
NVIC_SetPriority(L3GD20_SPI_INT2_EXTI_IRQn, 2);
/* толкнем петлю ДУС */
DUS_Zapros();
}
/* запретить работу петли ДУС */
void DUS_PetlyaDeInit(void)
{
__IO uint32_t L3GD20Timeout = L3GD20_FLAG_TIMEOUT;
/* запретим прерывания по линии готовности ДУС */
NVIC_DisableIRQ(L3GD20_SPI_INT2_EXTI_IRQn);
/* дождемся окончания ПДП */
while (GPIO_ReadInputDataBit(L3GD20_SPI_CS_GPIO_PORT,L3GD20_SPI_CS_PIN)==Bit_RESET){
if((L3GD20Timeout--) == 0) L3GD20_TIMEOUT_UserCallback();
}
/* запретим работу ПДП */
DMA_Cmd (DUS_DMA_Wr, DISABLE); /* сначала запретим запись(!!!!) */
DMA_Cmd (DUS_DMA_Rd, DISABLE);
/* запретим прерывания после ПДП */
NVIC_DisableIRQ(DMA1_Channel2_IRQn);
}
/* запрос данных ПДП */
void DUS_Zapros(void)
{
if (DMA_GetCurrDataCounter(DUS_DMA_Rd)){
/* запретим прерывания по линии готовности ДУС */
NVIC_DisableIRQ(L3GD20_SPI_INT2_EXTI_IRQn);
return;
}
/* установим выбор ведомого ус-ва */
L3GD20_CS_LOW();
/* сбросим флаг завершения цикла ПДП */
DMA_ClearFlag(DMA1_FLAG_GL2);DMA_ClearFlag(DMA1_FLAG_GL3);
/* настроим адрес и режим передачи */
DUSbuffer[0]= L3GD20_OUT_TEMP_ADDR|READWRITE_CMD|MULTIPLEBYTE_CMD;
/* перезарядка ПДП */
DMA_SetCurrDataCounter(DUS_DMA_Rd,9);
DMA_SetCurrDataCounter(DUS_DMA_Wr,9);
/* разрешаем работу ПДП */
DMA_Cmd (DUS_DMA_Rd, ENABLE); /* сначала разрешаем чтение(!!!!) */
DMA_Cmd (DUS_DMA_Wr, ENABLE);
}
void DUS_EndDMASPITrans_ISR(void)
{
/* запретим работу ПДП */
DMA_Cmd (DUS_DMA_Wr, DISABLE); /* сначала запретим запись(!!!!) */
DMA_Cmd (DUS_DMA_Rd, DISABLE);
/* снимем выбор ус-ва с ведомого */
L3GD20_CS_HIGH();
/* обновляем данные уc-в ИНС */
DUSDataRdy();
NVIC_EnableIRQ(L3GD20_SPI_INT2_EXTI_IRQn);
}
Код под Ф3, под Ф4 у меня нет.