Создание собственной системы стабилизации
6000 есть с донора, если нужно для опытов не вопрос.
компас надо либо вешать на спи либо понижать частоту айтуси
второй иму будет интересен, надо поискать другие свободные лапы.
юсб можно вообще убрать с мини платы. все вывести на мкроплощадки для распайки на тонкие проводки,
на микроаппарататах каждый грамм на счету кому нужен юсб припаяются
миниюсб вообще лошадь и не вариант
6000 есть с донора, если нужно для опытов не вопрос.
“Золушка в 12 часов превратилась в тыкву, но принца уже было не остановить” ))) я уже с ф4бы выдрал - всё пучком…
лап нет сапсем ))) - экспериментирую дальше )))
Всем здравия в новом году!
Привет! С Наступившим!
что компас в 9250 - нехорошее железо и связываться с ним не хочется
Делись, чем оно плохо? А то у меня есть платка с ней, но пока не разбирался.
а) вешать компас через 9250 или оставить через 6000 (это всё не проверено и недоказуемо)
б) вешать компас на spi (HMC5983) забирать обе лапы с 6000 - тем самым забыть о 2- imu
Вешать лучше отдельно (не через МПУ), ибо ты сам написал почему. хотя… надо бы проверить сквозное чтение.
2 датчика нужны? а зачем? если только в плане отказоустойчивости, но прецедентов в полёте ведь не было? если смысл удорожания и нагромождения?
- сделать плату чуть шире скажем не 28Х28 а 28Х32 - будет место поставить MINI-USB рядом с выходами на моторы, а не микро как сейчас…
вот мне понравилась компоновка 20х40 banggood.com/Mini-NAZE32-SP-Racing-F3-Flight-Contr…
но правда это Ф3, там назе32шная дурь с внешним юсб-юсарт, можно поставить Ф4 и убрать мостик, и ещё флэху спиашную поставить
Делись, чем оно плохо? А то у меня есть платка с ней, но пока не разбирался.
грубо: если данные с hmc мне надо делить на 1095, то АК надо умножать на 1.5 ( вчера всё-таки заставил 6000 -ю упираться в аукс и2ц - надо осцилл на работе забрать теперь…
и ещё флэху спиашную поставить
мы с Максом думали на воткнуть что-нибудь вместо sd, но чёт ничего серьёзного (в плане объёма) и дешевого нет - легче sd припаять))) в мелкой пока связка фрам+сд не мешают)))
недостатки сд карт.
-кардхолдер занимает много места
-содержит намагничиваемый металл и если рядом с компасом то после касания его отверткой может отравлять компасу жизнь
-содержит котнтакты нужно осторожничать с промывкой, может насосать в себя раствора промывочной жидкости и тогда привет
в логике арду заложено что если карта не вставлена при запуске то только консоль - значит нужен будет джампер при отказе от сд
в логике арду заложено что если карта не вставлена при запуске то только консоль - значит нужен будет джампер при отказе от сд
ну его - эту логику:
- если что-то не так вываливаться в консоль
- желательно при перепрошивке стирать фрам (задать буту такую возможность) что на мой взгляд лучше всего - никаких проблем с параметрами после прошивки…
- из станции стирать фрам с ребутом
например в таулабсе в отличии от опенпилота можно флеш стирать из станции, а не заливать отдельную прошивку для этого…
а так да особенно на мелкой - кусок железа на всё брюхо…
И так, HMC5883 пишется и читается через MPU6000, одна проблема - потерял ось X компаса (
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_MPU6000M MPU6000M Functions
* @brief Deals with the hardware interface to the 3-axis gyro
* @{
*
* @file pios_mpu6000m.c
* @author Swift-Flyer, , Copyright (C) 2015
* @brief MPU6000M 9-axis gyro accel and mag chip
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************
*/
/*
* 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, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_MPU6000M)
#include "physical_constants.h"
#include "pios_mpu6000m.h"
#include "pios_semaphore.h"
#include "pios_thread.h"
#include "pios_queue.h"
/* Private constants */
#define MPU6000M_TASK_PRIORITY PIOS_THREAD_PRIO_HIGHEST
#define MPU6000M_TASK_STACK_BYTES 512
#define PIOS_MPU6000M_MAX_DOWNSAMPLE 2
#define MPU6000M_WHOAMI_ID 0x68
#ifdef PIOS_MPU6000M_SPI_HIGH_SPEED
#define MPU6000M_SPI_HIGH_SPEED PIOS_MPU6000M_SPI_HIGH_SPEED
#else
#define MPU6000M_SPI_HIGH_SPEED 20000000 // should result in 10.5MHz clock on F4 targets like Sparky2
#endif
#define MPU6000M_SPI_LOW_SPEED 1000000
/* HMC5883 Addresses */
#define PIOS_MPU6000M_HMC5883_I2C_ADDR 0x1E
#define PIOS_MPU6000M_HMC5883_I2C_READ_ADDR 0x3D
#define PIOS_MPU6000M_HMC5883_I2C_WRITE_ADDR 0x3C
#define PIOS_MPU6000M_HMC5883_CONFIG_REG_A (uint8_t)0x00
#define PIOS_MPU6000M_HMC5883_CONFIG_REG_B (uint8_t)0x01
#define PIOS_MPU6000M_HMC5883_MODE_REG (uint8_t)0x02
#define PIOS_MPU6000M_HMC5883_DATAOUT_XMSB_REG 0x03
#define PIOS_MPU6000M_HMC5883_DATAOUT_XLSB_REG 0x04
#define PIOS_MPU6000M_HMC5883_DATAOUT_ZMSB_REG 0x05
#define PIOS_MPU6000M_HMC5883_DATAOUT_ZLSB_REG 0x06
#define PIOS_MPU6000M_HMC5883_DATAOUT_YMSB_REG 0x07
#define PIOS_MPU6000M_HMC5883_DATAOUT_YLSB_REG 0x08
#define PIOS_MPU6000M_HMC5883_DATAOUT_STATUS_REG 0x09
#define PIOS_MPU6000M_HMC5883_DATAOUT_IDA_REG 0x0A
#define PIOS_MPU6000M_HMC5883_DATAOUT_IDB_REG 0x0B
#define PIOS_MPU6000M_HMC5883_DATAOUT_IDC_REG 0x0C
/* Output Data Rate */
#define PIOS_MPU6000M_HMC5883_ODR_0_75 0x00
#define PIOS_MPU6000M_HMC5883_ODR_1_5 0x04
#define PIOS_MPU6000M_HMC5883_ODR_3 0x08
#define PIOS_MPU6000M_HMC5883_ODR_7_5 0x0C
#define PIOS_MPU6000M_HMC5883_ODR_15 0x10
#define PIOS_MPU6000M_HMC5883_ODR_30 0x14
#define PIOS_MPU6000M_HMC5883_ODR_75 0x18
/* Measure configuration */
#define PIOS_MPU6000M_HMC5883_MEASCONF_NORMAL 0x00
#define PIOS_MPU6000M_HMC5883_MEASCONF_BIAS_POS 0x01
#define PIOS_MPU6000M_HMC5883_MEASCONF_BIAS_NEG 0x02
/* Gain settings */
#define PIOS_MPU6000M_HMC5883_GAIN_0_88 0x00
#define PIOS_MPU6000M_HMC5883_GAIN_1_3 0x20
#define PIOS_MPU6000M_HMC5883_GAIN_1_9 0x40
#define PIOS_MPU6000M_HMC5883_GAIN_2_5 0x60
#define PIOS_MPU6000M_HMC5883_GAIN_4_0 0x80
#define PIOS_MPU6000M_HMC5883_GAIN_4_7 0xA0
#define PIOS_MPU6000M_HMC5883_GAIN_5_6 0xC0
#define PIOS_MPU6000M_HMC5883_GAIN_8_1 0xE0
/* Modes */
#define PIOS_MPU6000M_HMC5883_MODE_CONTINUOUS 0x00
#define PIOS_MPU6000M_HMC5883_MODE_SINGLE 0x01
#define PIOS_MPU6000M_HMC5883_MODE_IDLE 0x02
#define PIOS_MPU6000M_HMC5883_MODE_SLEEP 0x03
/* Global Variables */
enum pios_mpu6000m_dev_magic {
PIOS_MPU6000M_DEV_MAGIC = 0x9da9b3ed,
};
struct mpu6000m_dev {
uint32_t spi_id;
uint32_t slave_num;
enum pios_mpu60x0_accel_range accel_range;
enum pios_mpu60x0_range gyro_range;
struct pios_queue *gyro_queue;
struct pios_queue *accel_queue;
struct pios_queue *mag_queue;
struct pios_thread *TaskHandle;
struct pios_semaphore *data_ready_sema;
const struct pios_mpu6000m_cfg *cfg;
enum pios_mpu60x0_filter filter;
enum pios_mpu6000m_dev_magic magic;
};
//! Global structure for this device device
static struct mpu6000m_dev *dev;
//! Private functions
static struct mpu6000m_dev *PIOS_MPU6000M_alloc(const struct pios_mpu6000m_cfg *cfg);
static int32_t PIOS_MPU6000M_Validate(struct mpu6000m_dev *dev);
static void PIOS_MPU6000M_Task(void *parameters);
static uint8_t PIOS_MPU6000M_ReadReg(uint8_t reg);
static int32_t PIOS_MPU6000M_WriteReg(uint8_t reg, uint8_t data);
static int32_t PIOS_MPU6000M_ClaimBus(bool lowspeed);
static int32_t PIOS_MPU6000M_ReleaseBus(bool lowspeed);
/**
* @brief Allocate a new device
*/
static struct mpu6000m_dev *PIOS_MPU6000M_alloc(const struct pios_mpu6000m_cfg *cfg)
{
struct mpu6000m_dev *mpu6000m_dev;
mpu6000m_dev = (struct mpu6000m_dev *)PIOS_malloc(sizeof(*mpu6000m_dev));
if (!mpu6000m_dev)
return NULL;
mpu6000m_dev->magic = PIOS_MPU6000M_DEV_MAGIC;
mpu6000m_dev->accel_queue = PIOS_Queue_Create(PIOS_MPU6000M_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_accel_data));
if (mpu6000m_dev->accel_queue == NULL) {
PIOS_free(mpu6000m_dev);
return NULL;
}
mpu6000m_dev->gyro_queue = PIOS_Queue_Create(PIOS_MPU6000M_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_gyro_data));
if (mpu6000m_dev->gyro_queue == NULL) {
PIOS_Queue_Delete(dev->accel_queue);
PIOS_free(mpu6000m_dev);
return NULL;
}
if (cfg->use_magnetometer) {
mpu6000m_dev->mag_queue = PIOS_Queue_Create(PIOS_MPU6000M_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_mag_data));
if (mpu6000m_dev->mag_queue == NULL) {
PIOS_Queue_Delete(dev->accel_queue);
PIOS_Queue_Delete(dev->gyro_queue);
PIOS_free(mpu6000m_dev);
return NULL;
}
}
mpu6000m_dev->data_ready_sema = PIOS_Semaphore_Create();
if (mpu6000m_dev->data_ready_sema == NULL) {
PIOS_Queue_Delete(dev->accel_queue);
PIOS_Queue_Delete(dev->gyro_queue);
if (cfg->use_magnetometer)
PIOS_Queue_Delete(dev->mag_queue);
PIOS_free(mpu6000m_dev);
return NULL;
}
return mpu6000m_dev;
}
/**
* @brief Validate the handle to the device
* @returns 0 for valid device or -1 otherwise
*/
static int32_t PIOS_MPU6000M_Validate(struct mpu6000m_dev *dev)
{
if (dev == NULL)
return -1;
if (dev->magic != PIOS_MPU6000M_DEV_MAGIC)
return -2;
if (dev->spi_id == 0)
return -3;
return 0;
}
/**
* @brief Claim the SPI bus for the communications and select this chip
* \param[in] flag controls if low speed access for control registers should be used
* @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
*/
static int32_t PIOS_MPU6000M_ClaimBus(bool lowspeed)
{
if (PIOS_MPU6000M_Validate(dev) != 0)
return -1;
if (PIOS_SPI_ClaimBus(dev->spi_id) != 0)
return -2;
if (lowspeed)
PIOS_SPI_SetClockSpeed(dev->spi_id, MPU6000M_SPI_LOW_SPEED);
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
return 0;
}
/**
* @brief Release the SPI bus for the communications and end the transaction
* \param[in] must be true when bus was claimed in lowspeed mode
* @return 0 if successful
*/
static int32_t PIOS_MPU6000M_ReleaseBus(bool lowspeed)
{
if (PIOS_MPU6000M_Validate(dev) != 0)
return -1;
PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
if (lowspeed)
PIOS_SPI_SetClockSpeed(dev->spi_id, MPU6000M_SPI_HIGH_SPEED);
PIOS_SPI_ReleaseBus(dev->spi_id);
return 0;
}
/**
* @brief Read a register from MPU6000M
* @returns The register value
* @param reg[in] Register address to be read
*/
static uint8_t PIOS_MPU6000M_ReadReg(uint8_t reg)
{
uint8_t data;
PIOS_MPU6000M_ClaimBus(true);
PIOS_SPI_TransferByte(dev->spi_id, 0x80 | reg); // request byte
data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response
PIOS_MPU6000M_ReleaseBus(true);
return data;
}
/**
* @brief Writes one byte to the MPU6000M register
* \param[in] reg Register address
* \param[in] data Byte to write
* @returns 0 when success
*/
static int32_t PIOS_MPU6000M_WriteReg(uint8_t reg, uint8_t data)
{
if (PIOS_MPU6000M_ClaimBus(true) != 0)
return -1;
PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg);
PIOS_SPI_TransferByte(dev->spi_id, data);
PIOS_MPU6000M_ReleaseBus(true);
return 0;
}
/**
* @brief Writes one byte to the HMC5883 register using MPU6000M I2C master
* \param[in] reg Register address
* \param[in] data Byte to write
* @returns 0 when success
*/
static int32_t PIOS_MPU6000M_Mag_WriteReg(uint8_t reg, uint8_t data)
{
// we will use I2C SLV4 to manipulate with HMC5883 control registers
if (PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_REG_REG, reg) != 0)
return -1;
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_ADDR_REG, PIOS_MPU6000M_HMC5883_I2C_ADDR);
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_DO_REG, data);
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_CTRL_REG, PIOS_MPU60X0_I2CSLV_EN);
uint32_t timeout = 0;
// wait for I2C transaction done, use simple safety
// escape counter to prevent endless loop in case
// MPU6000M is broken
uint8_t status = 0;
do {
if (timeout++ > 50)
return -2;
status = PIOS_MPU6000M_ReadReg(PIOS_MPU60X0_I2C_MST_STATUS_REG);
} while ((status & PIOS_MPU60X0_I2C_MST_SLV4_DONE) == 0);
if (status & PIOS_MPU60X0_I2C_MST_SLV4_NACK)
return -3;
return 0;
}
/**
* @brief Reads one byte from the HMC5883 register using MPU6000M I2C master
* \param[in] reg Register address
* \param[in] data Byte to write
**/
static uint8_t PIOS_MPU6000M_Mag_ReadReg(uint8_t reg)
{
// we will use I2C SLV4 to manipulate with HMC5883 control registers
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_REG_REG, reg);
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_ADDR_REG, PIOS_MPU6000M_HMC5883_I2C_ADDR | 0x80);
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_CTRL_REG, PIOS_MPU60X0_I2CSLV_EN);
uint32_t timeout = 0;
// wait for I2C transaction done, use simple safety
// escape counter to prevent endless loop in case
// MPU6000M is broken
uint8_t status = 0;
do {
if (timeout++ > 50)
return 0;
status = PIOS_MPU6000M_ReadReg(PIOS_MPU60X0_I2C_MST_STATUS_REG);
} while ((status & PIOS_MPU60X0_I2C_MST_SLV4_DONE) == 0);
return PIOS_MPU6000M_ReadReg(PIOS_MPU60X0_SLV4_DI_REG);
}
/**
* @brief Initialize the HMC5883 magnetometer inside MPU6000M
* \return 0 if success
*
*/
static int32_t PIOS_MPU6000M_Mag_Config(void)
{
char idm = PIOS_MPU6000M_Mag_ReadReg(PIOS_MPU6000M_HMC5883_DATAOUT_IDA_REG);
if(idm != 'H') // Expect H
return -2;
// CRTL_REGA
PIOS_MPU6000M_Mag_WriteReg(PIOS_MPU6000M_HMC5883_CONFIG_REG_A, 0x70);
PIOS_DELAY_WaitmS(2);
// CRTL_REGB
PIOS_MPU6000M_Mag_WriteReg(PIOS_MPU6000M_HMC5883_CONFIG_REG_B, 0x20);
PIOS_DELAY_WaitmS(2);
// Mode register
PIOS_MPU6000M_Mag_WriteReg(PIOS_MPU6000M_HMC5883_MODE_REG, 0x00);
// give chip some time to initialize
PIOS_DELAY_WaitmS(2);
// configure mpu6000m to read hmc5x83 data range from register
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV0_REG_REG, PIOS_MPU6000M_HMC5883_DATAOUT_XMSB_REG);
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV0_ADDR_REG, PIOS_MPU6000M_HMC5883_I2C_ADDR | 0x80);
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV0_CTRL_REG, PIOS_MPU60X0_I2CSLV_EN | 6);
return 0;
}
/**
* @brief Read the identification bytes from the HMC5883 sensor
* \param[out] uint8_t array of size 4 to store HMC5883 ID.
* \return 0 if successful, -1 if not
static uint8_t PIOS_MPU6000M_HMC5883_ReadID(uint8_t out)
{
uint8_t retval = PIOS_MPU6000M_Mag_ReadReg(PIOS_MPU6000M_HMC5883_DATAOUT_IDA_REG, 0);
out = '\0';
return retval;
} */
/**
* @brief Initialize the MPU6000M gyro & accel registers
* \return 0 if successful
* \param[in] pios_mpu6000m_cfg struct to be used to configure sensor.
*
*/
static int32_t PIOS_MPU6000M_Config(struct pios_mpu6000m_cfg const *cfg)
{
// reset chip
if (PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_PWR_MGMT_REG, PIOS_MPU60X0_PWRMGMT_IMU_RST) != 0)
return -1;
// give chip some time to initialize
PIOS_DELAY_WaitmS(50);
uint8_t id = PIOS_MPU6000M_ReadReg(PIOS_MPU60X0_WHOAMI);
if (id != MPU6000M_WHOAMI_ID)
return -2;
// power management config
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_PWR_MGMT_REG, PIOS_MPU60X0_PWRMGMT_PLL_X_CLK);
// user control
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_USER_CTRL_REG, PIOS_MPU60X0_USERCTL_DIS_I2C | PIOS_MPU60X0_USERCTL_I2C_MST_EN);
if (dev->cfg->use_magnetometer)
if (PIOS_MPU6000M_Mag_Config() != 0)
return -3;
// Digital low-pass filter and scale
// set this before sample rate else sample rate calculation will fail
PIOS_MPU6000M_SetLPF(cfg->default_filter);
// Sample rate
PIOS_MPU6000M_SetSampleRate(cfg->default_samplerate);
// Set the gyro scale
PIOS_MPU6000M_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG);
// Set the accel scale
PIOS_MPU6000M_SetAccelRange(PIOS_MPU60X0_ACCEL_8G);
// Interrupt configuration
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_INT_CFG_REG, cfg->interrupt_cfg);
// Interrupt enable
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_INT_EN_REG, PIOS_MPU60X0_INTEN_DATA_RDY);
return 0;
}
/**
* @brief Initialize the MPU6000M 9-axis sensor.
* @return 0 for success, -1 for failure to allocate, -10 for failure to get irq
*/
int32_t PIOS_MPU6000M_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000m_cfg *cfg)
{
dev = PIOS_MPU6000M_alloc(cfg);
if (dev == NULL)
return -1;
dev->spi_id = spi_id;
dev->slave_num = slave_num;
dev->cfg = cfg;
/* Configure the MPU6000M Sensor */
if (PIOS_MPU6000M_Config(cfg) != 0)
return -2;
/* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg);
// Wait 20 ms for data ready interrupt and make sure it happens
// twice
if ((PIOS_Semaphore_Take(dev->data_ready_sema, 20) != true) ||
(PIOS_Semaphore_Take(dev->data_ready_sema, 20) != true)) {
return -10;
}
dev->TaskHandle = PIOS_Thread_Create(
PIOS_MPU6000M_Task, "pios_mpu6000m", MPU6000M_TASK_STACK_BYTES, NULL, MPU6000M_TASK_PRIORITY);
PIOS_Assert(dev->TaskHandle != NULL);
PIOS_SENSORS_Register(PIOS_SENSOR_ACCEL, dev->accel_queue);
PIOS_SENSORS_Register(PIOS_SENSOR_GYRO, dev->gyro_queue);
if (dev->cfg->use_magnetometer)
PIOS_SENSORS_Register(PIOS_SENSOR_MAG, dev->mag_queue);
return 0;
}
/**
* @brief Test MPU6000M presence on the bus
* @returns 0 if success
*/
int32_t PIOS_MPU6000M_Test(void)
{
uint8_t id = PIOS_MPU6000M_ReadReg(PIOS_MPU60X0_WHOAMI);
if (id != 0x68)
return 1;
char idm = PIOS_MPU6000M_Mag_ReadReg(PIOS_MPU6000M_HMC5883_DATAOUT_IDA_REG);
if(idm != 'H') // Expect H
return 1;
return 0;
}
/**
* @brief Set gyroscope range
* @returns 0 if successful
* @param range[in] gyroscope range
*/
void PIOS_MPU6000M_SetGyroRange(enum pios_mpu60x0_range gyro_range)
{
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_GYRO_CFG_REG, gyro_range);
switch (gyro_range) {
case PIOS_MPU60X0_SCALE_250_DEG:
PIOS_SENSORS_SetMaxGyro(250);
break;
case PIOS_MPU60X0_SCALE_500_DEG:
PIOS_SENSORS_SetMaxGyro(500);
break;
case PIOS_MPU60X0_SCALE_1000_DEG:
PIOS_SENSORS_SetMaxGyro(1000);
break;
case PIOS_MPU60X0_SCALE_2000_DEG:
PIOS_SENSORS_SetMaxGyro(2000);
break;
}
dev->gyro_range = gyro_range;
}
/**
* @brief Set accelerometer range
* @returns 0 if success
* @param range[in] accelerometer range
*/
void PIOS_MPU6000M_SetAccelRange(enum pios_mpu60x0_accel_range accel_range)
{
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_ACCEL_CFG_REG, accel_range);
dev->accel_range = accel_range;
}
/**
* @brief Set sampling frequency of accels and gyros axes
* @returns 0 if successful
* @param samplerate_hz[in] Sampling frequency in Hz
*/
void PIOS_MPU6000M_SetSampleRate(uint16_t samplerate_hz)
{
// mpu6000m ODR divider is unable to run from 8kHz clock like mpu60x0 :(
// check if someone want to use 250Hz DLPF and don't want 8kHz sampling
// and politely refuse him
uint16_t filter_frequency = 8000;
if (dev->filter != PIOS_MPU60X0_LOWPASS_256_HZ)
filter_frequency = 1000;
// limit samplerate to filter frequency
if (samplerate_hz > filter_frequency)
samplerate_hz = filter_frequency;
// calculate divisor, round to nearest integeter
int32_t divisor = (int32_t)(((float)filter_frequency / samplerate_hz) + 0.5f) - 1;
// limit resulting divisor to register value range
if (divisor < 0)
divisor = 0;
if (divisor > 0xff)
divisor = 0xff;
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SMPLRT_DIV_REG, (uint8_t)divisor);
}
/**
* Configure the digital low-pass filter
*/
void PIOS_MPU6000M_SetLPF(enum pios_mpu60x0_filter filter)
{
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_DLPF_CFG_REG, filter);
dev->filter = filter;
}
/**
* @brief Get current gyro scale for deg/s
* @returns scale
*/
static float PIOS_MPU6000M_GetGyroScale(void)
{
switch (dev->gyro_range) {
case PIOS_MPU60X0_SCALE_250_DEG:
return 1.0f / 131.0f;
case PIOS_MPU60X0_SCALE_500_DEG:
return 1.0f / 65.5f;
case PIOS_MPU60X0_SCALE_1000_DEG:
return 1.0f / 32.8f;
case PIOS_MPU60X0_SCALE_2000_DEG:
return 1.0f / 16.4f;
}
return 0;
}
/**
* @brief Get current gyro scale for ms^-2
* @returns scale
*/
static float PIOS_MPU6000M_GetAccelScale(void)
{
switch (dev->accel_range) {
case PIOS_MPU60X0_ACCEL_2G:
return GRAVITY / 16384.0f;
case PIOS_MPU60X0_ACCEL_4G:
return GRAVITY / 8192.0f;
case PIOS_MPU60X0_ACCEL_8G:
return GRAVITY / 4096.0f;
case PIOS_MPU60X0_ACCEL_16G:
return GRAVITY / 2048.0f;
}
return 0;
}
/**
* @brief IRQ Handler. Notice MPU6000M task to read all sensors data.
*/
bool PIOS_MPU6000M_IRQHandler(void)
{
if (PIOS_MPU6000M_Validate(dev) != 0)
return false;
bool need_yield = false;
PIOS_Semaphore_Give_FromISR(dev->data_ready_sema, &need_yield);
return need_yield;
}
static void PIOS_MPU6000M_Task(void *parameters)
{
while (1) {
//Wait for data ready interrupt
if (PIOS_Semaphore_Take(dev->data_ready_sema, PIOS_SEMAPHORE_TIMEOUT_MAX) != true)
continue;
enum {
IDX_REG = 0,
IDX_ACCEL_XOUT_H,
IDX_ACCEL_XOUT_L,
IDX_ACCEL_YOUT_H,
IDX_ACCEL_YOUT_L,
IDX_ACCEL_ZOUT_H,
IDX_ACCEL_ZOUT_L,
IDX_TEMP_OUT_H,
IDX_TEMP_OUT_L,
IDX_GYRO_XOUT_H,
IDX_GYRO_XOUT_L,
IDX_GYRO_YOUT_H,
IDX_GYRO_YOUT_L,
IDX_GYRO_ZOUT_H,
IDX_GYRO_ZOUT_L,
IDX_MAG_XOUT_L,
IDX_MAG_XOUT_H,
IDX_MAG_YOUT_L,
IDX_MAG_YOUT_H,
IDX_MAG_ZOUT_L,
IDX_MAG_ZOUT_H,
BUFFER_SIZE,
};
uint8_t mpu6000m_rec_buf[BUFFER_SIZE];
uint8_t mpu6000m_tx_buf[BUFFER_SIZE] = {PIOS_MPU60X0_ACCEL_X_OUT_MSB | 0x80, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t transfer_size = (dev->cfg->use_magnetometer) ? BUFFER_SIZE : BUFFER_SIZE - 6;
// claim bus in high speed mode
if (PIOS_MPU6000M_ClaimBus(false) != 0)
continue;
if (PIOS_SPI_TransferBlock(dev->spi_id, mpu6000m_tx_buf, mpu6000m_rec_buf, transfer_size, 0) < 0) {
PIOS_MPU6000M_ReleaseBus(false);
continue;
}
PIOS_MPU6000M_ReleaseBus(false);
struct pios_sensor_accel_data accel_data;
struct pios_sensor_gyro_data gyro_data;
struct pios_sensor_mag_data mag_data;
float accel_x = (int16_t)(mpu6000m_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000m_rec_buf[IDX_ACCEL_XOUT_L]);
float accel_y = (int16_t)(mpu6000m_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000m_rec_buf[IDX_ACCEL_YOUT_L]);
float accel_z = (int16_t)(mpu6000m_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000m_rec_buf[IDX_ACCEL_ZOUT_L]);
float gyro_x = (int16_t)(mpu6000m_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000m_rec_buf[IDX_GYRO_XOUT_L]);
float gyro_y = (int16_t)(mpu6000m_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000m_rec_buf[IDX_GYRO_YOUT_L]);
float gyro_z = (int16_t)(mpu6000m_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000m_rec_buf[IDX_GYRO_ZOUT_L]);
float mag_x = (int16_t)(mpu6000m_rec_buf[IDX_MAG_XOUT_H] << 8 | mpu6000m_rec_buf[IDX_MAG_XOUT_L]);
float mag_y = (int16_t)(mpu6000m_rec_buf[IDX_MAG_YOUT_H] << 8 | mpu6000m_rec_buf[IDX_MAG_YOUT_L]);
float mag_z = (int16_t)(mpu6000m_rec_buf[IDX_MAG_ZOUT_H] << 8 | mpu6000m_rec_buf[IDX_MAG_ZOUT_L]);
// Rotate the sensor to TL convention. The datasheet defines X as towards the right
// and Y as forward. TL convention transposes this. Also the Z is defined negatively
// to our convention. This is true for accels and gyros. Magnetometer corresponds TL convention.
switch (dev->cfg->orientation) {
case PIOS_MPU6000M_TOP_0DEG:
accel_data.y = accel_x;
accel_data.x = accel_y;
accel_data.z = -accel_z;
gyro_data.y = gyro_x;
gyro_data.x = gyro_y;
gyro_data.z = -gyro_z;
mag_data.x = mag_y;
mag_data.y = mag_x;
mag_data.z = -mag_z;
break;
case PIOS_MPU6000M_TOP_90DEG:
accel_data.y = -accel_y;
accel_data.x = accel_x;
accel_data.z = -accel_z;
gyro_data.y = -gyro_y;
gyro_data.x = gyro_x;
gyro_data.z = -gyro_z;
mag_data.x = -mag_x;
mag_data.y = mag_y;
mag_data.z = -mag_z;
break;
case PIOS_MPU6000M_TOP_180DEG:
accel_data.y = -accel_x;
accel_data.x = -accel_y;
accel_data.z = -accel_z;
gyro_data.y = -gyro_x;
gyro_data.x = -gyro_y;
gyro_data.z = -gyro_z;
mag_data.x = -mag_y;
mag_data.y = -mag_x;
mag_data.z = -mag_z;
break;
case PIOS_MPU6000M_TOP_270DEG:
accel_data.y = accel_y;
accel_data.x = -accel_x;
accel_data.z = -accel_z;
gyro_data.y = gyro_y;
gyro_data.x = -gyro_x;
gyro_data.z = -gyro_z;
mag_data.x = mag_x;
mag_data.y = -mag_y;
mag_data.z = -mag_z;
break;
case PIOS_MPU6000M_BOTTOM_0DEG:
accel_data.y = -accel_x;
accel_data.x = accel_y;
accel_data.z = accel_z;
gyro_data.y = -gyro_x;
gyro_data.x = gyro_y;
gyro_data.z = gyro_z;
mag_data.x = mag_y;
mag_data.y = -mag_x;
mag_data.z = mag_z;
break;
case PIOS_MPU6000M_BOTTOM_90DEG:
accel_data.y = -accel_y;
accel_data.x = -accel_x;
accel_data.z = accel_z;
gyro_data.y = -gyro_y;
gyro_data.x = -gyro_x;
gyro_data.z = gyro_z;
mag_data.x = -mag_x;
mag_data.y = -mag_y;
mag_data.z = mag_z;
break;
case PIOS_MPU6000M_BOTTOM_180DEG:
accel_data.y = accel_x;
accel_data.x = -accel_y;
accel_data.z = accel_z;
gyro_data.y = gyro_x;
gyro_data.x = -gyro_y;
gyro_data.z = gyro_z;
mag_data.x = -mag_y;
mag_data.y = mag_x;
mag_data.z = mag_z;
break;
case PIOS_MPU6000M_BOTTOM_270DEG:
accel_data.y = accel_y;
accel_data.x = accel_x;
gyro_data.y = gyro_y;
gyro_data.x = gyro_x;
gyro_data.z = gyro_z;
accel_data.z = accel_z;
mag_data.x = mag_x;
mag_data.y = mag_y;
mag_data.z = mag_z;
break;
}
int16_t raw_temp = (int16_t)(mpu6000m_rec_buf[IDX_TEMP_OUT_H] << 8 | mpu6000m_rec_buf[IDX_TEMP_OUT_L]);
float temperature = 21.0f + ((float)raw_temp) / 333.87f;
// Apply sensor scaling
float accel_scale = PIOS_MPU6000M_GetAccelScale();
accel_data.x *= accel_scale;
accel_data.y *= accel_scale;
accel_data.z *= accel_scale;
accel_data.temperature = temperature;
float gyro_scale = PIOS_MPU6000M_GetGyroScale();
gyro_data.x *= gyro_scale;
gyro_data.y *= gyro_scale;
gyro_data.z *= gyro_scale;
gyro_data.temperature = temperature;
PIOS_Queue_Send(dev->accel_queue, &accel_data, 0);
PIOS_Queue_Send(dev->gyro_queue, &gyro_data, 0);
if (dev->cfg->use_magnetometer) {
mag_data.x /= 1090.0f;
mag_data.y /= 1090.0f;
mag_data.z /= 1090.0f;
PIOS_Queue_Send(dev->mag_queue, &mag_data, 0);
}
}
}
#endif
/**
* @}
* @}
*/
и все это включая утилизацию мпу ради одной ноги
вместо того чтобы повесить 6000 и 5983 на одну spi с разными cs?
ноги 2 - ещё прерывание и вообще сейчас висит 9250 и 6000+5883(2 набора, 2 cs, 2 прерывания) на проце задействованы все лапы - все, нету ни одной))))
включая утилизацию мпу
я чё первый раз датчики ломаю?)))
ладно - надо понять где ось х делась…
ну так прерывания отродясь никто не использовал хотя все регулярно разводят. вот и две ноги. cs можно из 2 ног 4 девайса вытащить используя дешифратор но это лишняя микруха конечно
ms5811 на той же spi?
да
используя дешифратор
места нет
то что компас достаётся через мпу - это очень хорошо, значит можно повесить через 9250 (6500) - меньше разводить… и читать сразу пачкой, а то загляни туда - посмотри там, а тут мпу всё в кучу собирает…
безусловно пропустить через мпу шину насквозь с точки зрения программирования задача непростая и интересная и заслуживающая уважения…
но с точки зрения применения для полетника оперировать гланды чере гинекологию как то нетипично чтоли… а надо ли?
думаю не многие поняли суть решения, схему надо показывать
а чего там интересного? проц <-spi->mpu<-aux i2c->hmc
мпу вычитывает компас и складывает себе в регистры, сама без принуждения, мы потом из них всё забираем…
ноги 2 - ещё прерывание и вообще сейчас висит 9250 и 6000+5883(2 набора, 2 cs, 2 прерывания)
Серёг, прерывание на компасе нафиг не надо, тем более если читаешь через МПУ. Или у тебя чтение через режим обхода?
мпу вычитывает компас и складывает себе в регистры, сама без принуждения
Так зачем тебе прерывание? МПУшка всё равно по своей инициативе считает, одним кадром СПИ ты сразу все датчики получаешь. Да и компас, датчик не нуждающийся в интеграции, стало быть пропуск отсчета никак не скажется на точности работы ИНС.
Так зачем тебе прерывание? МПУшка всё равно по своей инициативе считает, одним кадром СПИ ты сразу все датчики получаешь.
да всё одним кадром, и прерывания с него нет, я про то - если вешать компас на SPI то не хватит одной лапы (cs) и придётся забыть о каком-то датчике…
проблема сейчас другая - где я ось потерял )))
все настройки мпу на “складирование” данных
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV0_REG_REG, PIOS_MPU6000M_HMC5883_DATAOUT_XMSB_REG); // откуда начинать читать
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV0_ADDR_REG, PIOS_MPU6000M_HMC5883_I2C_ADDR | 0x80); // читать пачку
PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV0_CTRL_REG, PIOS_MPU60X0_I2CSLV_EN | 6); // резервирование регистров для данных - куда складывать
т.е. всё по-порядку должно забираться
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_EXT_SENS_DATA_00 0x49 // это от компаса
#define MPUREG_EXT_SENS_DATA_01 0x4A
#define MPUREG_EXT_SENS_DATA_02 0x4B
#define MPUREG_EXT_SENS_DATA_03 0x4C
#define MPUREG_EXT_SENS_DATA_04 0x4D
#define MPUREG_EXT_SENS_DATA_05 0x4E
догнал надо частоту дискретизации с i2c править
I2C_SLV4_DLY_EN When enabled, slave 4 will only be accessed at a decreased rate.
I2C_SLV3_DLY_EN When enabled, slave 3 will only be accessed at a decreased rate.
I2C_SLV2_DLY_EN When enabled, slave 2 will only be accessed at a decreased rate.
I2C_SLV1_DLY_EN When enabled, slave 1 will only be accessed at a decreased rate.
I2C_SLV0_DLY_EN When enabled, slave 0 will only be accessed at a decreased rate.
Чёт с буржуйским туго
When DELAY_ES_SHADOW is set to 1, shadowing of external sensor data is delayed until all data has been received.
пока не получит все данные не сложит в регистры?
может это включает шадоу технологию, пока идет запись пишется в дублирующий регистр а чтение происходит из основного, как только дописалось регистры меняются местами
альтернатива - читаются данные как есть
Просто не могу понять, куда девается ось - тупо нули в 2-х регистрах, зацепил регистр статуса компаса - ничего не поменялось… возможно придётся регистры для отдельного чтения, id всякие, сдвигать принудительно, дабы регистры с 49 по 4f были чисто для данных с датчика…
был дохлый компас, все оси завелись - перепутал только ориентацию… надо повернуть на 180 гр. разберусь и в небо)))
видать когда мпу пеканул - компасу тоже досталось - дым знатный был т.к. всё это у меня висело на проводах 0.1 - отгорели почти все )))
видать когда мпу пеканул - компасу тоже досталось - дым знатный был т.к. всё это у меня висело на проводах 0.1 - отгорели почти все )))
Дым?😃 У тебя аккум на плату замкнул?
если на компас подать 5 вольт с ограничением по току ампера 3 то сгорает дотла. и на плате дырка прогорает