Создание собственной системы стабилизации
Т.е. фактически у Вас не дельта скорости на выходе а дельта ускорения…
в настоящее время беру и скорость и текущую позицию прямо из расчетов инерциалки
сохряняю текущую позицию и скрость в буффер “снизу” это проталкивает данные наверх, сэмплов 5, шестой затирается.
github.com/kozinalexey/…/AP_InertialNav.cpp#L255
_hp_x.push_back(_position.x); //save current inav position to buffer back point for calculate position change during gps delay
_hp_y.push_back(_position.y);
а тут я считаю разницу между тем что было по состоянию на момент актуальности текущих но запаздывающих жпс данных и тем что сейчас
github.com/kozinalexey/…/AP_InertialNav.cpp#L268
_gps_position_lag_x = _position.x - _hp_x.peek(_gps_sample_number); //peek 0 400ms peek 1 300ms etc
_gps_position_lag_y = _position.y - _hp_y.peek(_gps_sample_number);
но на быстрых процах можно было бы из ускорений все считать
Ну да ладно, а результат проверки этого алгоритма в полете можете прокоментировать ? лучше стало ? (насколько)…
визуально сильно снизилась тенденция к раскрутке по спирали при ошибке компаса, при анализе логов уменьшилось расхождение между координатами ГНСС и INAV. я вывел в лог в пакете INAV параметры LA LN с тем чтобы можно было сравнивать инерциальные координаты с данными навигационного приемника gps LAT LNG прямо в анализаторе логов мишен планера
Т.е. фактически у Вас не дельта скорости на выходе а дельта ускорения… а прибавляете вы ее, если не ошибаюсь, к расстоянию по жпс… как то не клеится (??)…
да нет, всё клеится РастИНС1-РастИНС0 = дельтаРастИНС, это как раз и есть расчет ИНСки между отсчетами ГНСС, т.е. актуализация положения между отсчетами ГНСС. дельтаРастИНС/dt = СкоростьИНС (за период dt); где dt - период получения данных ГНСС. Алексей, коэффициент притяжки ИНС и период обновления ИНС какие?
Алексей, коэффициент притяжки ИНС и период обновления ИНС какие?
_gps_k_gps_spd =0.003 к 0.997 (подтяг скорости )
_gps_k_gps_pos =0.008 к 0.992 (подтяг позиции )
осуществляется с частотой около 100 гц
_position.x = _position.x * _gps_k_inav_pos + (_gps_position.x + _gps_position_lag_x) * _gps_k_gps_pos; //pool inav position to gps position
_position.y = _position.y * _gps_k_inav_pos + (_gps_position.y + _gps_position_lag_y) * _gps_k_gps_pos;
_velocity.x = _velocity.x * _gps_k_inav_spd + (_gps_velocity_x + _gps_velocity_lag_x) * _gps_k_gps_spd ; //pool inav velocity to gps velocity
_velocity.y = _velocity.y * _gps_k_inav_spd + (_gps_velocity_y + _gps_velocity_lag_y) * _gps_k_gps_spd ;
но имеет смысл после внедрения компенсациипопробовать подтяг скорости усилить до 0.005 к 0.995
параметры подтяга и компенсации жпс лага - ругулируемые из парметров, привел значения которые были использованы в полете на видео
к раскрутке по спирали при ошибке компаса,
компас, я так понял, смешивается с гирой и акселем в общий алгоритм ориентации ИНС (типа алго магвика и махони)??
визуально сильно снизилась тенденция к раскрутке
тогда - это результат (!), поздравляю…
Я тут был воодушевлён применением RPi в качестве контроллера полета… Так вот последние изыскания показали, что при работе с SPI многозадачный линукс становится фактически однозадачным… Иными словами - запустив в одном терминале процесс обмена данными по SPI и их выводом на экран, другие задачи запускаться не собираются до тех пор пока мы не убьём этот процесс…
Вывод: нафига тогда всё это (?), и вопрос по передаче видео по WiFi и тому подобное отпадает сам собой… Не понятно как люди, делающие платы типа “NAVIO” надеются на развитие этого направления (?).
Там правда у них какойто “патч” линукса… и похоже I2C… - но это, по моему, только усугубляет проблему…
Так вот последние изыскания показали, что при работе с SPI многозадачный линукс становится фактически однозадачным… Иными словами - запустив в одном терминале процесс обмена данными по SPI и их выводом на экран, другие задачи запускаться не собираются до тех пор пока мы не убьём этот процесс…
Дык там похоже внутри процесса бесконечный опрос битов в регистре СПИ, надо код драйвера прошерстить, в момент ожидания должна вызываться спецфункция передачи управления ОС.
надо код драйвера прошерстить,
Да, тут без “хирургического вмешательства” не обойтись, только для такого меня этот процесс может надолго затянуться… проще наверно дождаться когда “гурманы-операционьщики” сделают нормальную РТОС для этих целей.
имхо напрасное это дело возлагать на бортовой компьютер функции по стабилизации полета и управлении моторами.
куда более было бы просто оставить стабилизацию на хорошем проверенном апм а функции навигации реализовать на бортовом компе
втч визуальную ориентацию. требуемые для полета угловые поправки можно передавать по мавлинку через сериал
а функции навигации реализовать на бортовом компе
втч визуальную ориентацию.
Всю навигацию, любого уровня сложности, stm обсчитывает не напрягаясь… Я у себя ему и RTOS и OSD навесил + 500 гц реалтайм, а ему (stmF4) всё ни по чём, только ОЗУ уже впритык… А визуальная ориентация - больше мечты, чем реальность, пока даже прототипов не видно на просторах инета.
От RPi хотелось только одного - чтоб весь софт был на языке высокого уровня, чтоб без всякой компиляции/перепрошивки и прочей колготы с программаторами и средами разработки, чуть ли не в поле, испытывать те или иные идеи алгоритмов управления, навешивать новые сенсоры, и т.д … пока что глухо с этой затеей (на полку, до лучших времен)
Всю навигацию, любого уровня сложности, stm обсчитывает не напрягаясь…
за исключением визуального ориентирования, именно его я и имел ввиду под внешней навигационной системой.
Привет Друзья и Коллеги по хобби! Пользуясь случаем поздравляю всех с наступающим Новым годом! Здоровья крепкого Вам и родным, удачи во всех начинаниях, мирного неба! Высоких и красивых полётов!
С наступающим Новым Годом!
Всем здравия в новом году!
Шавет нужен:
Короче писал-писал драйвер да не выписал, точнее драйвер то есть, но случилось беда - при экспериментах с мелкой уложил 6000 и лапу проца - но не суть, куплю - приедут. Как говорится “плохой результат-тоже результат” )))
Суть в другом, пока это всё ехать собирается, на мелкой накопилось куча косяков и вопросов которые надо решать:
- и самое основное - что делать? (риторический вопрос) т.к. я-мы убедились, что компас в 9250 - нехорошее железо и связываться с ним не хочется - есть 2 решения данной беды:
а) вешать компас через 9250 или оставить через 6000 (это всё не проверено и недоказуемо)
б) вешать компас на spi (HMC5983) забирать обе лапы с 6000 - тем самым забыть о 2- imu 😦 - сделать плату чуть шире скажем не 28Х28 а 28Х32 - будет место поставить MINI-USB рядом с выходами на моторы, а не микро как сейчас…
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?