Специальные цены   новые товары
+ Ответить в теме
Страница 46 из 165 ПерваяПервая ... 36 44 45 46 47 48 56 ... ПоследняяПоследняя
Показано с 1,801 по 1,840 из 6569

Создание собственной системы стабилизации

Тема раздела Квадрокоптеры. Общие вопросы в категории Квадрокоптеры и мультироторы; Сообщение от Probelzaelo А хуже всего что каждый норовит предложить свой вариант перевода Эта потому что переводят не спецы в ...

  1. #1801

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от Probelzaelo Посмотреть сообщение
    А хуже всего что каждый норовит предложить свой вариант перевода
    Эта потому что переводят не спецы в предметной области, главное бабла нарубить, бардакс ...


    Цитата Сообщение от SergDoc Посмотреть сообщение
    Ну так что? есть два варианта: эмулятор и внешняя, чему отдать предпочтение?
    Для настроек однозначно внутренняя память, для логов SPI. А так нужно бы знать , что сделать прощще.

  2.  
  3. #1802

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Не пойму, задаю:
    Код:
    #ifndef FLASH_PAGE_COUNT
    #define FLASH_PAGE_COUNT 4 //128X4=512
    #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                 ((uint16_t)0x20000) // размер 7-го сектора
    #define FLASH_WRITE_ADDR                (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 1)) // начальный адрес виртуальной eeprom
    а эта гадость сносит мне первый сектор
    так уже не стираем, но блин пишем в первый сектор....
    ну вот не пишем не стираем, а проц вообще пропал, только в dfu поймать можно
    Последний раз редактировалось SergDoc; 09.04.2013 в 16:12.

  4. #1803

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Что то ерунда какаято, поставь сразу #define FLASH_WRITE_ADDR ADDR_FLASH_SECTOR_3
    и исключи этот сектор из сегмента IROM, линкёр сам разбросает код вокруг этого сегмента.

  5. #1804

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    я с седьмым сектором это сделал, но по ходу в него не пишется, я уже на работе вечером ещё поизголяюсь...

    Цитата Сообщение от rual Посмотреть сообщение
    IROM
    Это где?

    может я ему на седьмой сектор мало напруги на стирание дал? voltage range 3
    Последний раз редактировалось SergDoc; 09.04.2013 в 16:40.

  6.  
  7. #1805

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    Это где?
    В кейле это тут


    Нажмите на изображение для увеличения
Название: keil.JPG
Просмотров: 45
Размер:	59.7 Кб
ID:	778575
    в ГЦЦ это в мэйк-файле наверно
    Цитата Сообщение от SergDoc Посмотреть сообщение
    может я ему на седьмой сектор мало напруги на стирание дал? voltage range 3
    Это я не знаю, щас с твоей подачи копаюсь в стандартных библиотеках, сообщу как накопаю.

  8. #1806

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Цитата Сообщение от rual Посмотреть сообщение
    В кейле это тут
    т.е. я пишу например IROM1 0x08000000 - траля-ля до нужного сектора
    IROM2 адрес последующего сектора - и до победы (до конца флеши)

    там по моему заковырка или первых три сектора или 5,6,7 сектора, может выделить ему первый сектор?

  9. #1807

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    там по моему заковырка или первых три сектора

    Надо доку на проц читать, надо посмотреть инициализацию счетчика команд и размещение таблицы векторов. я пока под Ф3 ваяю.

    Цитата Сообщение от SergDoc Посмотреть сообщение
    т.е. я пишу например IROM1 0x08000000 - траля-ля до нужного сектора IROM2 адрес последующего сектора - и до победы (до конца флеши)

    всё верно
    Последний раз редактировалось rual; 09.04.2013 в 18:30.

  10.  
  11. #1808

    Регистрация
    26.11.2012
    Адрес
    Tambov
    Возраст
    46
    Сообщений
    777
    Цитата Сообщение от rual Посмотреть сообщение
    Олег, не понятно какие библиотеки используете, см. в архиве стандартные функции
    Стандартные функции смотрел, происходит вообще лажа какаэто, по многочисленным экспериментам имею следующие "явления":
    1.корректно записывается только первый (любой) регистр конфигурации следующий уже сбоит...
    2.при исполнении SPI_SendData8(L3GD20_SPI, byte) с байтом типа "на запись" дождаться флага RXNE невозможно (будто с гиры байт не приходит)..
    3.При понижении (!!!) скорости SPI ниже "SPI_BaudRatePrescaler_8" вообще ничего не читается и не пишется..
    Ну и наконец самое чудное (ударение на "о"), при чтении ось Z не читается а на ее месте данные по оси Y....
    Очень похоже что дело в железе.
    Менял чипы два раза, проверил "печатку", пока результат "0" , копаюсь.....

  12. #1809

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    осима секретные данные
    drv_spi
    Код:
    /**
     * MultiWii NG 0.1 - 2012
     * HAL for STM32f3Discovery: SPI
     *
     * 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 <http://www.gnu.org/licenses/>.
     */
    
    #ifndef DRV_SPI_H_INCLUDED
    #define DRV_SPI_H_INCLUDED
    
    DMA_InitTypeDef  DMA_InitStruct;
    
    inline void init_spi() {
      /* Enable the SPI periph */
      RCC_APB2PeriphClockCmd(L3GD20_SPI_CLK, ENABLE);
      /* Enable SCK, MOSI and MISO GPIO clocks */
      RCC_AHBPeriphClockCmd(L3GD20_SPI_SCK_GPIO_CLK | L3GD20_SPI_MISO_GPIO_CLK | L3GD20_SPI_MOSI_GPIO_CLK, ENABLE);
      /* Enable CS  GPIO clock */
      RCC_AHBPeriphClockCmd(L3GD20_SPI_CS_GPIO_CLK, ENABLE);
      /* Enable INT1 GPIO clock */
      RCC_AHBPeriphClockCmd(L3GD20_SPI_INT1_GPIO_CLK, ENABLE);
      /* Enable INT2 GPIO clock */
      RCC_AHBPeriphClockCmd(L3GD20_SPI_INT2_GPIO_CLK, ENABLE);
    
      GPIO_PinAFConfig(L3GD20_SPI_SCK_GPIO_PORT, L3GD20_SPI_SCK_SOURCE, L3GD20_SPI_SCK_AF);
      GPIO_PinAFConfig(L3GD20_SPI_MISO_GPIO_PORT, L3GD20_SPI_MISO_SOURCE, L3GD20_SPI_MISO_AF);
      GPIO_PinAFConfig(L3GD20_SPI_MOSI_GPIO_PORT, L3GD20_SPI_MOSI_SOURCE, L3GD20_SPI_MOSI_AF);
    
      GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
      GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
      GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;//GPIO_PuPd_DOWN;
      GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
      /* SPI SCK pin configuration */
      GPIO_InitStructure.GPIO_Pin = L3GD20_SPI_SCK_PIN;
      GPIO_Init(L3GD20_SPI_SCK_GPIO_PORT, &GPIO_InitStructure);
      /* SPI  MOSI pin configuration */
      GPIO_InitStructure.GPIO_Pin =  L3GD20_SPI_MOSI_PIN;
      GPIO_Init(L3GD20_SPI_MOSI_GPIO_PORT, &GPIO_InitStructure);
      /* SPI MISO pin configuration */
      GPIO_InitStructure.GPIO_Pin = L3GD20_SPI_MISO_PIN;
      GPIO_Init(L3GD20_SPI_MISO_GPIO_PORT, &GPIO_InitStructure);
      /* SPI configuration -------------------------------------------------------*/
      SPI_I2S_DeInit(L3GD20_SPI);
      SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
      SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
      SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
      SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
      SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
      SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16;
      SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
      SPI_InitStructure.SPI_CRCPolynomial = 7;
      SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
      SPI_Init(L3GD20_SPI, &SPI_InitStructure);
      /* Configure the RX FIFO Threshold */
      SPI_RxFIFOThresholdConfig(L3GD20_SPI, SPI_RxFIFOThreshold_QF);
      /* Enable SPI1  */
      SPI_Cmd(L3GD20_SPI, ENABLE);
      /* Configure GPIO PIN for Lis Chip select */
      GPIO_InitStructure.GPIO_Pin = L3GD20_SPI_CS_PIN;
      GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
      GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
      GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
      GPIO_Init(L3GD20_SPI_CS_GPIO_PORT, &GPIO_InitStructure);
      /* Deselect : Chip Select high */
      GPIO_SetBits(L3GD20_SPI_CS_GPIO_PORT, L3GD20_SPI_CS_PIN);
      /* Setup DMA */
      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 = 0;
      DMA_InitStruct.DMA_BufferSize =  0;
      /* write */
      DMA_InitStruct.DMA_Priority = DMA_Priority_Medium;
      DMA_InitStruct.DMA_DIR = DMA_DIR_PeripheralDST;
      DMA_Init(DMA1_Channel3, &DMA_InitStruct);
      /* read */
      DMA_InitStruct.DMA_Priority = DMA_Priority_High;
      DMA_InitStruct.DMA_DIR = DMA_DIR_PeripheralSRC;
      DMA_Init(DMA1_Channel2, &DMA_InitStruct);
      /* enable DMA */
      SPI_I2S_DMACmd(L3GD20_SPI, SPI_I2S_DMAReq_Rx, ENABLE);
      SPI_I2S_DMACmd(L3GD20_SPI, SPI_I2S_DMAReq_Tx, ENABLE);
    }
    
    void _start_spi_dma(uint8_t* pBuffer, uint16_t NumByteToWrite) {
      // Setup buffer
      DMA1_Channel2->CMAR = (uint32_t)pBuffer;
      DMA1_Channel3->CMAR = (uint32_t)pBuffer;
      DMA1_Channel2->CNDTR = NumByteToWrite;
      DMA1_Channel3->CNDTR = NumByteToWrite;
      //
      DMA_ClearFlag(DMA1_FLAG_GL2);
      DMA_ClearFlag(DMA1_FLAG_GL3);
      /* start */
      DMA_Cmd(DMA1_Channel2, ENABLE);
      DMA_Cmd(DMA1_Channel3, ENABLE);
    }
    
    void _disable_spi_dma() {
      DMA_Cmd(DMA1_Channel2, DISABLE);
      DMA_Cmd(DMA1_Channel3, DISABLE);
    }
    
    static uint8_t _spi_xchange(uint8_t byte) {
      while (SPI_I2S_GetFlagStatus(L3GD20_SPI, SPI_I2S_FLAG_TXE) == RESET);
      SPI_SendData8(L3GD20_SPI, byte);
      while (SPI_I2S_GetFlagStatus(L3GD20_SPI, SPI_I2S_FLAG_RXNE) == RESET);
      while (SPI_I2S_GetFlagStatus(L3GD20_SPI, SPI_I2S_FLAG_BSY) == SET);
      return (uint8_t)SPI_ReceiveData8(L3GD20_SPI);
    }
    
    void spi_write_buff(uint8_t* buff, uint8_t reg, uint16_t size) {
      if(size > 0x01) reg |= (uint8_t)MULTIPLEBYTE_CMD;
      _spi_xchange(reg);
      while(size >= 0x01) {
        _spi_xchange(*buff);
        size--; buff++;
      }
    }
    
    inline void spi_write_byte(uint8_t reg, uint8_t val) {
      spi_write_buff(&val, reg, 1);
    }
    
    void spi_read_buff(uint8_t* buff, uint8_t reg, uint16_t size) {
      reg |= (uint8_t)READWRITE_CMD;
      if(size > 0x01) reg |= (uint8_t)(MULTIPLEBYTE_CMD);
      _spi_xchange(reg);
      while(size > 0x00) {
        *buff = _spi_xchange(0x00);
        size--; buff++;
      }
    }
    
    inline uint8_t spi_read_byte(uint8_t reg) {
      uint8_t buff;
      spi_read_buff(&buff, reg, 1);
      return buff;
    }
    
    static PT_THREAD(spi_read_buffer_pt(struct pt *pt, uint8_t reg, uint8_t *buff, uint8_t size)) {
      PT_BEGIN(pt);
      PT_WAIT_WHILE(pt, (SPI_I2S_GetFlagStatus(L3GD20_SPI, SPI_I2S_FLAG_TXE) == RESET));
      reg |= (uint8_t)READWRITE_CMD;
      if(size > 0x01) reg |= (uint8_t)(MULTIPLEBYTE_CMD);
      SPI_SendData8(L3GD20_SPI, reg);
      PT_WAIT_WHILE(pt, (SPI_I2S_GetFlagStatus(L3GD20_SPI, SPI_I2S_FLAG_RXNE) == RESET));
      PT_WAIT_WHILE(pt, (SPI_I2S_GetFlagStatus(L3GD20_SPI, SPI_I2S_FLAG_BSY) == SET));
      SPI_ReceiveData8(L3GD20_SPI);
      _start_spi_dma(buff, size);
      PT_WAIT_WHILE(pt, DMA_GetCurrDataCounter(DMA1_Channel2));
      _disable_spi_dma();
      PT_END(pt);
    }
    
    
    #endif // DRV_SPI_H_INCLUDED
    stm32f3_discovery_l3gd20
    Код:
    /**
      ******************************************************************************
      * @file    stm32f3_discovery_l3gd20.h
      * @author  MCD Application Team
      * @version V1.1.0
      * @date    20-September-2012
      * @brief   This file contains definitions for stm32f3_discovery_l3gd20.c 
      *          firmware driver.
      ******************************************************************************
      * @attention
      *
      * <h2><center>&copy; COPYRIGHT 2012 STMicroelectronics</center></h2>
      *
      * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
      * You may not use this file except in compliance with the License.
      * You may obtain a copy of the License at:
      *
      *        http://www.st.com/software_license_agreement_liberty_v2
      *
      * Unless required by applicable law or agreed to in writing, software 
      * distributed under the License is distributed on an "AS IS" BASIS, 
      * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      * See the License for the specific language governing permissions and
      * limitations under the License.
      *
      ******************************************************************************
      */
      
    
    /* Define to prevent recursive inclusion -------------------------------------*/
    #ifndef __STM32F3_DISCOVERY_L3GD20_H
    #define __STM32F3_DISCOVERY_L3GD20_H
    
    #ifdef __cplusplus
     extern "C" {
    #endif
    
    /* Includes ------------------------------------------------------------------*/
     #include "stm32f30x.h"
    
    /** @addtogroup Utilities
      * @{
      */
      
    /** @addtogroup STM32F3_DISCOVERY
      * @{
      */ 
    
    /** @addtogroup STM32F3_DISCOVERY_L3GD20
      * @{
      */
      
    /** @defgroup STM32F3_DISCOVERY_L3GD20_Exported_Types
      * @{
      */
    
    /* L3GD20 struct */
    typedef struct
    {
      uint8_t Power_Mode;                         /* Power-down/Sleep/Normal Mode */
      uint8_t Output_DataRate;                    /* OUT data rate */
      uint8_t Axes_Enable;                        /* Axes enable */
      uint8_t Band_Width;                         /* Bandwidth selection */
      uint8_t BlockData_Update;                   /* Block Data Update */
      uint8_t Endianness;                         /* Endian Data selection */
      uint8_t Full_Scale;                         /* Full Scale selection */
    }L3GD20_InitTypeDef;
    
    /* L3GD20 High Pass Filter struct */
    typedef struct
    {
      uint8_t HighPassFilter_Mode_Selection;      /* Internal filter mode */
      uint8_t HighPassFilter_CutOff_Frequency;    /* High pass filter cut-off frequency */
    }L3GD20_FilterConfigTypeDef;
    
    /* L3GD20 Interrupt struct */
    typedef struct
    {
      uint8_t Latch_Request;                      /* Latch interrupt request into CLICK_SRC register */
      uint8_t Interrupt_Axes;                     /* X, Y, Z Axes Interrupts */ 
      uint8_t Interrupt_ActiveEdge;               /*  Interrupt Active edge */
    }L3GD20_InterruptConfigTypeDef;  
    
    /**
      * @}
      */ 
    
    
    /**
      * @}
      */
      
    /** @defgroup STM32F3_DISCOVERY_L3GD20_Exported_Constants
      * @{
      */
    
    /* Read/Write command */
    #define READWRITE_CMD              ((uint8_t)0x80) 
    /* Multiple byte read/write command */ 
    #define MULTIPLEBYTE_CMD           ((uint8_t)0x40)
    /* Dummy Byte Send by the SPI Master device in order to generate the Clock to the Slave device */
    #define DUMMY_BYTE                 ((uint8_t)0x00)
    
    /* Uncomment the following line to use the default L3GD20_TIMEOUT_UserCallback() 
       function implemented in stm32f3_discovery_lgd20.c file.
       L3GD20_TIMEOUT_UserCallback() function is called whenever a timeout condition 
       occure during communication (waiting transmit data register empty flag(TXE)
       or waiting receive data register is not empty flag (RXNE)). */   
    /* #define USE_DEFAULT_TIMEOUT_CALLBACK */
    
    /* Maximum Timeout values for flags waiting loops. These timeouts are not based
       on accurate values, they just guarantee that the application will not remain
       stuck if the SPI communication is corrupted.
       You may modify these timeout values depending on CPU frequency and application
       conditions (interrupts routines ...). */   
    #define L3GD20_FLAG_TIMEOUT             ((uint32_t)0x1000)
    
    /**
      * @brief  L3GD20 SPI Interface pins
      */
    #define L3GD20_SPI                       SPI1
    #define L3GD20_SPI_CLK                   RCC_APB2Periph_SPI1
    
    #define L3GD20_SPI_SCK_PIN               GPIO_Pin_5                  /* PA.05 */
    #define L3GD20_SPI_SCK_GPIO_PORT         GPIOA                       /* GPIOA */
    #define L3GD20_SPI_SCK_GPIO_CLK          RCC_AHBPeriph_GPIOA
    #define L3GD20_SPI_SCK_SOURCE            GPIO_PinSource5
    #define L3GD20_SPI_SCK_AF                GPIO_AF_5
    
    #define L3GD20_SPI_MISO_PIN              GPIO_Pin_6                  /* PA.6 */
    #define L3GD20_SPI_MISO_GPIO_PORT        GPIOA                       /* GPIOA */
    #define L3GD20_SPI_MISO_GPIO_CLK         RCC_AHBPeriph_GPIOA
    #define L3GD20_SPI_MISO_SOURCE           GPIO_PinSource6
    #define L3GD20_SPI_MISO_AF               GPIO_AF_5
    
    #define L3GD20_SPI_MOSI_PIN              GPIO_Pin_7                  /* PA.7 */
    #define L3GD20_SPI_MOSI_GPIO_PORT        GPIOA                       /* GPIOA */
    #define L3GD20_SPI_MOSI_GPIO_CLK         RCC_AHBPeriph_GPIOA
    #define L3GD20_SPI_MOSI_SOURCE           GPIO_PinSource7
    #define L3GD20_SPI_MOSI_AF               GPIO_AF_5
    
    #define L3GD20_SPI_CS_PIN                GPIO_Pin_3                  /* PE.03 */
    #define L3GD20_SPI_CS_GPIO_PORT          GPIOE                       /* GPIOE */
    #define L3GD20_SPI_CS_GPIO_CLK           RCC_AHBPeriph_GPIOE
    
    #define L3GD20_SPI_INT1_PIN              GPIO_Pin_0                  /* PE.00 */
    #define L3GD20_SPI_INT1_GPIO_PORT        GPIOE                       /* GPIOE */
    #define L3GD20_SPI_INT1_GPIO_CLK         RCC_AHBPeriph_GPIOE
    #define L3GD20_SPI_INT1_EXTI_LINE        EXTI_Line0
    #define L3GD20_SPI_INT1_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE
    #define L3GD20_SPI_INT1_EXTI_PIN_SOURCE  EXTI_PinSource0
    #define L3GD20_SPI_INT1_EXTI_IRQn        EXTI0_IRQn 
    
    #define L3GD20_SPI_INT2_PIN              GPIO_Pin_1                  /* PE.01 */
    #define L3GD20_SPI_INT2_GPIO_PORT        GPIOE                       /* GPIOE */
    #define L3GD20_SPI_INT2_GPIO_CLK         RCC_AHBPeriph_GPIOE
    #define L3GD20_SPI_INT2_EXTI_LINE        EXTI_Line1
    #define L3GD20_SPI_INT2_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE
    #define L3GD20_SPI_INT2_EXTI_PIN_SOURCE  EXTI_PinSource1
    #define L3GD20_SPI_INT2_EXTI_IRQn        EXTI1_IRQn 
    
    /******************************************************************************/
    /*************************** START REGISTER MAPPING  **************************/
    /******************************************************************************/
    #define L3GD20_WHO_AM_I_ADDR          0x0F  /* device identification register */
    #define L3GD20_CTRL_REG1_ADDR         0x20  /* Control register 1 */
    #define L3GD20_CTRL_REG2_ADDR         0x21  /* Control register 2 */
    #define L3GD20_CTRL_REG3_ADDR         0x22  /* Control register 3 */
    #define L3GD20_CTRL_REG4_ADDR         0x23  /* Control register 4 */
    #define L3GD20_CTRL_REG5_ADDR         0x24  /* Control register 5 */
    #define L3GD20_REFERENCE_REG_ADDR     0x25  /* Reference register */
    #define L3GD20_OUT_TEMP_ADDR          0x26  /* Out temp register */
    #define L3GD20_STATUS_REG_ADDR        0x27  /* Status register */
    #define L3GD20_OUT_X_L_ADDR           0x28  /* Output Register X */
    #define L3GD20_OUT_X_H_ADDR           0x29  /* Output Register X */
    #define L3GD20_OUT_Y_L_ADDR           0x2A  /* Output Register Y */
    #define L3GD20_OUT_Y_H_ADDR           0x2B  /* Output Register Y */
    #define L3GD20_OUT_Z_L_ADDR           0x2C  /* Output Register Z */
    #define L3GD20_OUT_Z_H_ADDR           0x2D  /* Output Register Z */ 
    #define L3GD20_FIFO_CTRL_REG_ADDR     0x2E  /* Fifo control Register */
    #define L3GD20_FIFO_SRC_REG_ADDR      0x2F  /* Fifo src Register */
    
    #define L3GD20_INT1_CFG_ADDR          0x30  /* Interrupt 1 configuration Register */
    #define L3GD20_INT1_SRC_ADDR          0x31  /* Interrupt 1 source Register */
    #define L3GD20_INT1_TSH_XH_ADDR       0x32  /* Interrupt 1 Threshold X register */
    #define L3GD20_INT1_TSH_XL_ADDR       0x33  /* Interrupt 1 Threshold X register */
    #define L3GD20_INT1_TSH_YH_ADDR       0x34  /* Interrupt 1 Threshold Y register */
    #define L3GD20_INT1_TSH_YL_ADDR       0x35  /* Interrupt 1 Threshold Y register */
    #define L3GD20_INT1_TSH_ZH_ADDR       0x36  /* Interrupt 1 Threshold Z register */
    #define L3GD20_INT1_TSH_ZL_ADDR       0x37  /* Interrupt 1 Threshold Z register */
    #define L3GD20_INT1_DURATION_ADDR     0x38  /* Interrupt 1 DURATION register */
    
    /******************************************************************************/
    /**************************** END REGISTER MAPPING  ***************************/
    /******************************************************************************/
    
    #define I_AM_L3GD20		    ((uint8_t)0xD4)
    
    /** @defgroup Power_Mode_selection 
      * @{
      */
    #define L3GD20_MODE_POWERDOWN       ((uint8_t)0x00)
    #define L3GD20_MODE_ACTIVE          ((uint8_t)0x08)
    /**
      * @}
      */
    
    /** @defgroup OutPut_DataRate_Selection 
      * @{
      */
    #define L3GD20_OUTPUT_DATARATE_1    ((uint8_t)0x00)
    #define L3GD20_OUTPUT_DATARATE_2    ((uint8_t)0x40)
    #define L3GD20_OUTPUT_DATARATE_3    ((uint8_t)0x80)
    #define L3GD20_OUTPUT_DATARATE_4    ((uint8_t)0xC0)
    /**
      * @}
      */
    
    /** @defgroup Axes_Selection 
      * @{
      */
    #define L3GD20_X_ENABLE            ((uint8_t)0x02)
    #define L3GD20_Y_ENABLE            ((uint8_t)0x01)
    #define L3GD20_Z_ENABLE            ((uint8_t)0x04)
    #define L3GD20_AXES_ENABLE         ((uint8_t)0x07)
    #define L3GD20_AXES_DISABLE        ((uint8_t)0x00)
    /**
      * @}
      */
    
    /** @defgroup BandWidth_Selection 
      * @{
      */
    #define L3GD20_BANDWIDTH_1         ((uint8_t)0x00)
    #define L3GD20_BANDWIDTH_2         ((uint8_t)0x10)
    #define L3GD20_BANDWIDTH_3         ((uint8_t)0x20)
    #define L3GD20_BANDWIDTH_4         ((uint8_t)0x30)
    /**
      * @}
      */
    
    /** @defgroup Full_Scale_Selection 
      * @{
      */
    #define L3GD20_FULLSCALE_250               ((uint8_t)0x00)
    #define L3GD20_FULLSCALE_500               ((uint8_t)0x10)
    #define L3GD20_FULLSCALE_2000              ((uint8_t)0x20) 
    /**
      * @}
      */
      
    /** @defgroup Block_Data_Update 
      * @{
      */  
    #define L3GD20_BlockDataUpdate_Continous   ((uint8_t)0x00)
    #define L3GD20_BlockDataUpdate_Single      ((uint8_t)0x80)
    /**
      * @}
      */
      
    /** @defgroup Endian_Data_selection
      * @{
      */  
    #define L3GD20_BLE_LSB                     ((uint8_t)0x00)
    #define L3GD20_BLE_MSB	                   ((uint8_t)0x40)
    /**
      * @}
      */
      
    /** @defgroup High_Pass_Filter_status 
      * @{
      */   
    #define L3GD20_HIGHPASSFILTER_DISABLE      ((uint8_t)0x00)
    #define L3GD20_HIGHPASSFILTER_ENABLE	     ((uint8_t)0x10)
    /**
      * @}
      */
    
    /** @defgroup INT1_Interrupt_status 
      * @{
      */   
    #define L3GD20_INT1INTERRUPT_DISABLE       ((uint8_t)0x00)
    #define L3GD20_INT1INTERRUPT_ENABLE	   ((uint8_t)0x80)
    /**
      * @}
      */
    
    /** @defgroup INT2_Interrupt_status 
      * @{
      */   
    #define L3GD20_INT2INTERRUPT_DISABLE       ((uint8_t)0x00)
    #define L3GD20_INT2INTERRUPT_ENABLE	   ((uint8_t)0x08)
    /**
      * @}
      */
    
    /** @defgroup INT1_Interrupt_ActiveEdge 
      * @{
      */   
    #define L3GD20_INT1INTERRUPT_LOW_EDGE      ((uint8_t)0x20)
    #define L3GD20_INT1INTERRUPT_HIGH_EDGE     ((uint8_t)0x00)
    /**
      * @}
      */
      
    /** @defgroup Boot_Mode_selection 
      * @{
      */
    #define L3GD20_BOOT_NORMALMODE             ((uint8_t)0x00)
    #define L3GD20_BOOT_REBOOTMEMORY           ((uint8_t)0x80)
    /**
      * @}
      */  
     
    /** @defgroup High_Pass_Filter_Mode 
      * @{
      */   
    #define L3GD20_HPM_NORMAL_MODE_RES         ((uint8_t)0x00)
    #define L3GD20_HPM_REF_SIGNAL              ((uint8_t)0x10)
    #define L3GD20_HPM_NORMAL_MODE             ((uint8_t)0x20)
    #define L3GD20_HPM_AUTORESET_INT           ((uint8_t)0x30)
    /**
      * @}
      */
    
    /** @defgroup High_Pass_CUT OFF_Frequency 
      * @{
      */   
    #define L3GD20_HPFCF_0              0x00
    #define L3GD20_HPFCF_1              0x01
    #define L3GD20_HPFCF_2              0x02
    #define L3GD20_HPFCF_3              0x03
    #define L3GD20_HPFCF_4              0x04
    #define L3GD20_HPFCF_5              0x05
    #define L3GD20_HPFCF_6              0x06
    #define L3GD20_HPFCF_7              0x07
    #define L3GD20_HPFCF_8              0x08
    #define L3GD20_HPFCF_9              0x09
    /**
      * @}
      */
    
    
    /** @defgroup STM32F3_DISCOVERY_L3GD20_Exported_Macros
      * @{
      */
    #define L3GD20_CS_LOW()       GPIO_ResetBits(L3GD20_SPI_CS_GPIO_PORT, L3GD20_SPI_CS_PIN)
    #define L3GD20_CS_HIGH()      GPIO_SetBits(L3GD20_SPI_CS_GPIO_PORT, L3GD20_SPI_CS_PIN)
    /**
      * @}
      */
     
    /** @defgroup STM32F3_DISCOVERY_L3GD20_Exported_Functions
      * @{
      */
    /* Sensor Configuration Functions */ 
    void L3GD20_Init(L3GD20_InitTypeDef *L3GD20_InitStruct);
    void L3GD20_RebootCmd(void);
    
    /*INT1 Interrupt Configuration Functions */
    void L3GD20_INT1InterruptCmd(uint8_t InterruptState);
    void L3GD20_INT2InterruptCmd(uint8_t InterruptState);
    void L3GD20_INT1InterruptConfig(L3GD20_InterruptConfigTypeDef *L3GD20_IntConfigStruct);
    uint8_t L3GD20_GetDataStatus(void);
    
    /* High Pass Filter Configuration Functions */
    void L3GD20_FilterConfig(L3GD20_FilterConfigTypeDef *L3GD20_FilterStruct);
    void L3GD20_FilterCmd(uint8_t HighPassFilterState);
    void L3GD20_Write(uint8_t* pBuffer, uint8_t WriteAddr, uint16_t NumByteToWrite);
    void L3GD20_Read(uint8_t* pBuffer, uint8_t ReadAddr, uint16_t NumByteToRead);
    
    /* USER Callbacks: This is function for which prototype only is declared in
       MEMS accelerometre driver and that should be implemented into user applicaiton. */  
    /* L3GD20_TIMEOUT_UserCallback() function is called whenever a timeout condition 
       occure during communication (waiting transmit data register empty flag(TXE)
       or waiting receive data register is not empty flag (RXNE)).
       You can use the default timeout callback implementation by uncommenting the 
       define USE_DEFAULT_TIMEOUT_CALLBACK in stm32f3_discovery_l3gd20.h file.
       Typically the user implementation of this callback should reset MEMS peripheral
       and re-initialize communication or in worst case reset all the application. */
    uint32_t L3GD20_TIMEOUT_UserCallback(void);
    
    #ifdef __cplusplus
    }
    #endif
    
    #endif /* __STM32F3_DISCOVERY_L3GD20_H */
    /**
      * @}
      */
    
    /**
      * @}
      */
    
    /**
      * @}
      */
    
    /**
      * @}
      */ 
    
    
    /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
    по моемУ - выделил нулевой сектор - прокатило, теперь застрял на инициализации АЦП (и вроде же из примера брал)... не что-то не так с виртуальником..... не пишет оно туда...
    Последний раз редактировалось SergDoc; 09.04.2013 в 21:31.

  13. #1810

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    в общем, флеш если отключать или использовать не занятую прошивкой - оно не работает, работает если занято, следовательно надо зарезервировать часть флеши в прошивке, вот как это сделать в f4 непонятно, примеры просто супер коварные...

  14. #1811

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Блин совсем уже крыша поехала, если стирать в указанной области - то стирает гдебы она не была, а вот записывать только может где данные во флеши, т.е. указываем например 0-й сектор (там у нас сама программа) - перезапишет сколько успеет, а если в пустом секторе-висяк конкретный, надо каким-то образом сектор включить в ПО и ff-ками заполнить... мыслей уже ноль...
    аа сам дурак, а кто в переменные глядеть должен, в 16-ти битную ну никак не влезет hex 20000, ну надо теперь всё перепроверять...
    Последний раз редактировалось SergDoc; 10.04.2013 в 00:16.

  15. #1812

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Цитата Сообщение от oleg70 Посмотреть сообщение
    Вот что у меня осталось от DiscoveryF3
    Олег, попробуйте на всякий случай повесить резистор помиж кондёрами на кварц (между лапами кварца) килоОм 100-150...

  16. #1813

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    поясните кто-нибудь эту строчку:
    Код:
     memcpy(&mcfg, (char *)FLASH_WRITE_ADDR, sizeof(master_t));
    отсюда http://code.google.com/p/afrodevices...t/src/config.c

  17. #1814

    Регистрация
    22.08.2011
    Адрес
    Калининград
    Возраст
    35
    Сообщений
    945
    Записей в дневнике
    2
    копать от сюда и до заката =))
    Приводим значение адреса в указатель на адрес памяти и копируем с этого адреса.

  18. #1815

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от oleg70 Посмотреть сообщение
    Стандартные функции смотрел,
    Олег, вот стандартная инициализация ДУС через стандартные драйверы дисковери (ранее я прилагал архив), см. внимательно, там же есть функции чтения и записи ИММЕНО по Ф3дисковери.
    Код:
    void DUS_Config(void)
    {
     L3GD20_InitTypeDef L3GD20_InitStructure;
     L3GD20_FilterConfigTypeDef L3GD20_FilterStructure;
     /* Configure Mems L3GD20 */
     L3GD20_InitStructure.Power_Mode = L3GD20_MODE_ACTIVE;
     L3GD20_InitStructure.Output_DataRate = L3GD20_OUTPUT_DATARATE_3;
     L3GD20_InitStructure.Axes_Enable = L3GD20_AXES_ENABLE;
     L3GD20_InitStructure.Band_Width = L3GD20_BANDWIDTH_4;
     L3GD20_InitStructure.BlockData_Update = L3GD20_BlockDataUpdate_Continous;
     L3GD20_InitStructure.Endianness = L3GD20_BLE_LSB;
     L3GD20_InitStructure.Full_Scale = L3GD20_FULLSCALE_2000; 
     L3GD20_Init(&L3GD20_InitStructure);
     
     L3GD20_FilterStructure.HighPassFilter_Mode_Selection =L3GD20_HPM_NORMAL_MODE_RES;
     L3GD20_FilterStructure.HighPassFilter_CutOff_Frequency = L3GD20_HPFCF_3;
     L3GD20_FilterConfig(&L3GD20_FilterStructure) ;
     L3GD20_FilterCmd(L3GD20_HIGHPASSFILTER_ENABLE);
     L3GD20_INT2InterruptCmd(L3GD20_INT2INTERRUPT_ENABLE);
    }

    Цитата Сообщение от SergDoc Посмотреть сообщение
    кто в переменные глядеть должен, в 16-ти битную ну никак не влезет
    пишем исключительно полусловами (16 бит), вот моя перекладка демки
    Код:
    /*------------------- ýìóëÿòîð ôëýø ------------------------*/
    FLASH_Status FLASH_Write16(uint32_t addr, uint16_t* buf, uint32_t size)
    {
     FLASH_Status FLASHStatus = FLASH_COMPLETE;
     uint32_t inx = 0;
     uint32_t EraseCounter = 0x00, Address = 0x00;  
      uint32_t NbrOfPage = 0x00;
     
     /* ïðîâåðèì âûõîä çà ïðåäåëû ðàçðåøåííîé îáëàñòè */
     if (addr+size*2 > FLASH_USER_END_ADDR) return FLASH_ERROR_PROGRAM;
     
      /* Ðàçáëîêèðîâêà äîñòóïà ê çàïèñè ôëåø */ 
      FLASH_Unlock();
     
      /* Î÷èñòèì ôëàãè */  
      FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); 
      /* Îïðåäåëèì êîëè÷åñòâî ñòèðàåìûõ ñòðàíèö */
      NbrOfPage = 1 + size / FLASH_PAGE_SIZE;
      /* Ñòèðàåì ñòðàíèöû */
      for(EraseCounter = 0; (EraseCounter < NbrOfPage) && (FLASHStatus == FLASH_COMPLETE); EraseCounter++)
      {
      FLASHStatus = FLASH_ErasePage(FLASH_USER_START_ADDR + (FLASH_PAGE_SIZE * EraseCounter));
        if (FLASHStatus != FLASH_COMPLETE)  return FLASHStatus;
      }
     
      /* Ïðîãðàììèðóåì ôëåø */
     
      Address = FLASH_USER_START_ADDR;
      while (inx < size) {
      FLASHStatus = FLASH_ProgramHalfWord(Address, buf[inx]);
     
        if (FLASHStatus == FLASH_COMPLETE){
          Address += 2;
       inx++;
      }  
        else return FLASHStatus;   
      }
      /* Çàáëîêèðóåì çàïèñü âî ôëåø */
      FLASH_Lock(); 
     
     return FLASHStatus;
    }
    Вот стандартная функция записи полуслова
    Код:
    /**
      * @brief  Programs a half word at a specified address.
      * @note   To correctly run this function, the FLASH_Unlock() function
      *         must be called before.
      *         Call the FLASH_Lock() to disable the flash memory access 
      *         (recommended to protect the FLASH memory against possible unwanted operation) 
      * @param  Address: specifies the address to be programmed.
      * @param  Data: specifies the data to be programmed.
      * @retval FLASH Status: The returned value can be: FLASH_ERROR_PG,
      *         FLASH_ERROR_WRP, FLASH_COMPLETE or FLASH_TIMEOUT. 
      */
    FLASH_Status FLASH_ProgramHalfWord(uint32_t Address, uint16_t Data)
    {
      FLASH_Status status = FLASH_COMPLETE;
      /* Check the parameters */
      assert_param(IS_FLASH_PROGRAM_ADDRESS(Address));
      /* Wait for last operation to be completed */
      status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT);
     
      if(status == FLASH_COMPLETE)
      {
        /* If the previous operation is completed, proceed to program the new data */
        FLASH->CR |= FLASH_CR_PG;
     
        *(__IO uint16_t*)Address = Data;
        /* Wait for last operation to be completed */
        status = FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT);
     
        /* Disable the PG Bit */
        FLASH->CR &= ~FLASH_CR_PG;
      }
    У меня всё отлично заработало, писал в старший сектор, чтоб линкёр в него не лез уменьшил верхнюю границу флеши в настройке проекта. Почитаю про ф4, напишу отличия...

  19. #1816

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    урааааа у меня зажегся первый светодиод(проверочный) !!!

    Цитата Сообщение от rual Посмотреть сообщение
    пишем исключительно полусловами (16 бит), вот моя перекладка демки
    я имел ввиду адрес сектора отрезанного под еепромку
    ну с одной мёртвой точки сдвинулся, пойду дальше копать....

  20. #1817

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    урааааа у меня зажегся первый светодиод(проверочный) !!!
    Поздравляю! С флешей разобрался? Вообщем от Ф3 практически не отличается, кроме возможности установить ширину данных, и ещё не увидел в описании процедуры программирования снятие бита программирования
    PG bit in the FLASH_CR register, надо смотреть стандартные библиотеки
    Цитата Сообщение от SergDoc Посмотреть сообщение
    я имел ввиду адрес сектора отрезанного под еепромку
    ну тут без вариантов uint32_t

  21. #1818

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    так у меня проблема - проц запускается на частоте - 53.76 Мгц - чё я где утварил?
    Виноват, сам скопировал от Discovery system_stm32f4xx.c и забыл про это....
    Последний раз редактировалось SergDoc; 10.04.2013 в 23:35.

  22. #1819

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Цитата Сообщение от rual Посмотреть сообщение
    и ещё не увидел в описании процедуры программирования снятие бита программирования
    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);
      }
    Цитата Сообщение от rual Посмотреть сообщение
    У меня всё отлично заработало, писал в старший сектор, чтоб линкёр в него не лез уменьшил верхнюю границу флеши в настройке проекта.
    посмотри st-link видит эту область, а то когда я по незнанке сносил нулевой сектор, то видел что он стирается и в него пишется, а так нет, а отпаивать, что-бы выдрать ввесь дамп, ну не хочется - при запаяном программаторе через uart не достучишься... ага вспомнил, если бы с виртуальником были бы траблы, он бы мне сообщил - светодиод бы моргал часто-часто, да и дальше бы не пошел...
    сейчас дошел до автодетекта (его выкинуть надо), а так как нечего детектировать то висим перемигиваемся светодиодиками - почему и заметил, что как-то не так моргают, полез частоту проверять...

  23. #1820

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    тут? stm32f4xx_flash.c
    Да, ту всё нормально, а в доке на Ф3 нет ничего про снятие бита программирования.


    Цитата Сообщение от SergDoc Посмотреть сообщение
    посмотри st-link видит эту область,

    СТ-ЛИНК видит-читает, но непосредственно править в отладчике не даёт, только через программирование.

  24. #1821

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    да-да у меня тоже всё прекрасно записано - отладчиком сейчас заглянул в седьмой сектор 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;
    }

  25. #1822

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    занято только примерно пол килобайта, ещё 127,5 - свободны

    Жалко тереть такую страницу, лучше всёж попытаться уйти в мелкий сектор.

  26. #1823

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    ну пока не напрягает, у меня всего кода на 68к... хотя можно смело спрыгнуть в первый сектор (не нулевой)..

  27. #1824

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Сергей, у тебя к этому порту ГУЙ Вийский или что-то своё?

  28. #1825

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Вий но пока до него не добрался.. в вингуи сделали сразу терминал, разберусь с датчиками буду пробовать цепляться..
    тут обновил
    Последний раз редактировалось SergDoc; 11.04.2013 в 14:05.

  29. #1826

    Регистрация
    13.03.2011
    Адрес
    Montreal, Canada
    Возраст
    38
    Сообщений
    2,290
    Записей в дневнике
    19
    Запускаем .NET MicroFramework на STM32F4Discovery

    ток в комментах пишут, что перформанс совсем дохленький...

  30. #1827

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Добрался до гиро-акселя, пока присматриваю откуда спилить, вопрос такого плана DMA на SPI1 (там мпу висит) использовать?

  31. #1828

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    вопрос такого плана DMA на SPI1 (там мпу висит) использовать?
    Конечно использовать, это пзволит избежать затрат времени на тупую пересылку байт. Вообще можно по готовности ДУСа читтать весь набор регистров в буфер, а потом по необходимости программно разбирать.

  32. #1829

    Регистрация
    26.11.2012
    Адрес
    Tambov
    Возраст
    46
    Сообщений
    777
    Буду очень благодарен, если кто нибудь доходчиво пояснит ЧТО и КАК фильтрует HIGH PASS Filter в датчиках, его практическое применение, в переводе с даташита так и не понял...

  33. #1830

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    получается как-то, не слишком хорошо, 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;

    картинко
    Миниатюры Миниатюры Нажмите на изображение для увеличения
Название: DMA2.jpg‎
Просмотров: 29
Размер:	65.3 Кб
ID:	779994  

  34. #1831

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Цитата Сообщение от oleg70 Посмотреть сообщение
    ЧТО и КАК фильтрует HIGH PASS Filter
    высокочастотный фильтр, пропускающий частоты выше некоторой частоты среза и ослабляющий более низкие частоты....
    ну типа решили мы полетать в дождь, а у нас две-три капли в секунду барабанят (ну в моём случае по салатнице) - аксель с ума сводят - можно обрезать данное мероприятие (к гироскопам не применимо)...
    о только щас подумал, а нафига акселю вообще вибрации чувствовать - зарезать с двух сторон - пусть доплывает потиху?

  35. #1832

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Цитата Сообщение от SergDoc Посмотреть сообщение
    Stream 3 Channel 3 - SPI1_TX;
    а это наверно и ненадо, зачем мне буфер на выход если я один раз при инициализации записываю данные?

  36. #1833

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    это наверно и ненадо, зачем мне буфер на выход если я один раз при инициализации записываю данные?
    Не факт, а адрес в начале запроса данных передавать не нужно?

    Вот вроде удобоворимо доделал прошивку для Ф3, может блином летать, все эксперементальные функции отключены. Настраивается через терминал в кодировке вин1251, клавишами со стрелками и ввода, там вроде всё понятно.
    Вложения

  37. #1834

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Цитата Сообщение от rual Посмотреть сообщение
    Не факт, а адрес в начале запроса данных передавать не нужно?
    ну да чёт я как-то не подумал....

  38. #1835

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Наверно без помощи я тут застряну, передача - тут можно и последовательно регистры программировать - засылать адрес регистра плюс по 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);
    Последний раз редактировалось SergDoc; 13.04.2013 в 23:24.

  39. #1836

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Сергей, 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 у меня нет.

  40. #1837

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Цитата Сообщение от rual Посмотреть сообщение
    Код под Ф3
    Я так понял что периферия очень похожа не на F4 а на F10x, ну эт не так страшно, только вот я не усмотрел - а канал какой в DMA или как оно в f4 Stream и Channel ? ладно ещё покумекаю, есть что-то такое:
    Код:
    //program initiates SPI1 and confirms that polling works.
    #define   tstBufferSize      5
    GPIO_SetBits(GPIOA, GPIO_Pin_4); //Set CS Off
    SPI_Cmd(SPI1, DISABLE);
     
    uint16_t SPI_MASTER_Buffer_Rx[tstBufferSize] = {99,99,99,99,99};
    uint16_t SPI_MASTER_Buffer_Tx[tstBufferSize] = {0x8000, 0, 0x7E00,  0x8003,  0};
     
    RCC_AHB1PeriphResetCmd(RCC_AHB1Periph_DMA2, ENABLE);
    DMA_Cmd(DMA2_Stream0, DISABLE);
    DMA_Cmd(DMA2_Stream5, DISABLE);
     
    /* SPI_MASTER_Rx_DMA_Channel configuration ---------------------------------*/
    DMA_DeInit(DMA2_Stream0);
    DMA_InitTypeDef DMA_InitStructure;
    DMA_StructInit(&DMA_InitStructure);
    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(SPI1_BASE + 0xC); //not sure about this
    DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)SPI_MASTER_Buffer_Rx;
    DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
    DMA_InitStructure.DMA_BufferSize = tstBufferSize;
    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
    DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
    DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
    DMA_InitStructure.DMA_Priority = DMA_Priority_High;
    DMA_InitStructure.DMA_Channel = DMA_Channel_3;
    DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
    //  DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
    DMA_Init(DMA2_Stream0, &DMA_InitStructure);
     
    /* SPI_MASTER_Tx_DMA_Channel configuration ---------------------------------*/
    DMA_DeInit(DMA2_Stream5);
    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(SPI1_BASE + 0xC); //not sure about this
    DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)SPI_MASTER_Buffer_Tx;
    DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
    DMA_InitStructure.DMA_Priority = DMA_Priority_Low;
    DMA_Init(DMA2_Stream5, &DMA_InitStructure);
     
     
    /* Enable SPI_MASTER DMA Tx request */
    SPI_I2S_DMACmd(SPI1, SPI_I2S_DMAReq_Tx, ENABLE);
    /* Enable SPI_MASTER DMA Rx request */
    SPI_I2S_DMACmd(SPI1, SPI_I2S_DMAReq_Rx, ENABLE);
     
    SPI_I2S_ClearFlag(SPI1, SPI_I2S_FLAG_TXE);
    SPI_I2S_ClearFlag(SPI1, SPI_I2S_FLAG_RXNE);
    GPIO_ResetBits(GPIOA, GPIO_Pin_4); //Set CS On
    /* Enable SPI_MASTER */
    SPI_Cmd(SPI1, ENABLE);
     
    /* Enable DMA channels */
    DMA_Cmd(DMA2_Stream0, ENABLE);
    DMA_Cmd(DMA2_Stream5, ENABLE);
     
    //while(!DMA_GetFlagStatus(DMA2_Stream0, DMA_FLAG_TCIF0));
    while(!DMA_GetFlagStatus(DMA2_Stream5, DMA_FLAG_TCIF5));
    GPIO_SetBits(GPIOA, GPIO_Pin_4); //Set CS Off
     
    while (1)
    {
    }
    надо только адаптировать....

  41. #1838

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    Я так понял что периферия очень похожа не на F4 а на F10x, ну эт не так страшно, только вот я не усмотрел - а канал какой в DMA или как оно в f4 Stream и Channel ?
    Да, Ф3 почти как Ф1. По твоему коду видно что в Ф4 в матрицу ПДП добавлено ещё одно измеренение -Stream.
    Что по сути ничего не меняет, в Ф1 просто выбирался канал, а в Ф4 выбирается поток (Stream)с набором каналов (Channel, задается в структуреDMA_InitStructure.DMA_Channel = DMA_Channel_3 .

  42. #1839

    Регистрация
    26.11.2012
    Адрес
    Tambov
    Возраст
    46
    Сообщений
    777
    Кто нибудь "сырые" данные с LSM303DLHC смотрел ?
    Что то у меня он "зверски" шумит (именно аксель, магнетометр приемлемо), буквально данные скачут +/- 16, 32, 64 мл.разр. при любой уст. чувствительности.....
    Читаю по прерыванию DRDY, причем при некоторых комбинациях datarate/full-scale вообще подвешивает I2C...

  43. #1840

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,460
    Записей в дневнике
    8
    Цитата Сообщение от rual Посмотреть сообщение
    Stream
    на каждый dma - 8 stream со своими 8-ю каналами в каждом, я там выше табличку выкладывал для dma2 т.к. вся моя периферия висит именно на нём...

    Цитата Сообщение от oleg70 Посмотреть сообщение
    буквально данные скачут +/- 16, 32, 64 мл.разр. при любой уст. чувствительности.....
    ну если вся шкала +-32767 то не так уж и страшно, в вие есть код под 303-ю...
    так-же в вие сделано подгоняем 1g =512 и подобран делитель для сырых данных дабы оно соответствовало 512 ещё аксель влючен на 8g- дабы не было переполнения...
    у меня сейчас в 330-стоит 400Гц и частота среза 50, но по прикидкам можно наамноого ниже резать.... взять что-нибудь такое 50-25 например...

+ Ответить в теме

Похожие темы

  1. Система стабилизации гиро+акселерометр
    от Фантомас в разделе Полеты по камере, телеметрия
    Ответов: 32
    Последнее сообщение: 25.01.2011, 14:47
  2. Продам Продам Клона Trex 450SEV2 + Аппаратура + Запчасти+ система стабилизации RTF
    от omegapraim в разделе Барахолка. Вертолеты
    Ответов: 1
    Последнее сообщение: 12.01.2011, 18:16
  3. Продам Трёхосевую систему стабилизации Turnigy V-Bar 600
    от avi@tor в разделе Барахолка. Аппаратура
    Ответов: 1
    Последнее сообщение: 08.11.2010, 13:02
  4. Продам Gaui система стабилизации GU365, дёшево.
    от avi@tor в разделе Барахолка. Вертолеты
    Ответов: 3
    Последнее сообщение: 03.08.2010, 11:13
  5. Системы стабилизации
    от max815 в разделе Фото и видеосъемка, системы стабилизации
    Ответов: 16
    Последнее сообщение: 11.03.2010, 03:14

Метки этой темы

Ваши права

  • Вы не можете создавать новые темы
  • Вы не можете отвечать в темах
  • Вы не можете прикреплять вложения
  • Вы не можете редактировать свои сообщения