Создание собственной системы стабилизации
Все настроил, но самолетики не движутся, у меня углы в переменных типа “int16_t”, как грамотно printf() подправить или тип данных для него ?
Все настроил, но самолетики не движутся
Контроллер долже вываливать вот такое
!ANG:-0.50,1.05,-69.99,0.00,-0.00,-85.50,-0.08,0.01,0.98,0.42,-0.00,-0.91,-0.00,0.01,-0.00
!ANG:-0.48,1.06,-70.00,0.00,-0.00,-85.50,-0.03,0.02,0.98,0.42,-0.01,-0.91,-0.01,0.01,-0.01
!ANG:-0.47,1.08,-70.01,0.00,-0.00,-85.50,-0.02,0.00,1.02,0.42,-0.00,-0.91,-0.01,0.02,0.00
!ANG:-0.48,1.06,-70.01,0.00,-0.00,-85.50,0.01,-0.02,1.03,0.43,0.01,-0.90,-0.01,0.01,0.01
!ANG:-0.50,1.05,-70.01,0.00,-0.00,-85.50,0.05,-0.02,1.02,0.43,-0.00,-0.91,0.00,0.01,0.01
!ANG:-0.50,1.05,-70.00,0.00,-0.00,-85.50,-0.01,-0.01,0.98,0.42,-0.00,-0.91,-0.00,0.00,0.01
Либо вот такое в минимальном формате
!ANG:-0.50,1.05,-69.99
!ANG:-0.48,1.06,-70.00
!ANG:-0.47,1.08,-70.01
Ещё нужно прописать свой ком-порт и скорость
ser = serial.Serial(port=‘COM5’,baudrate=57600, timeout=1)
И ещё момент, в конце строки ЧЕРЕЗ запятую ставь символ “>”? т.к. у меня по началу проверяется ответ контроллера
Таймкоп что-то раздобрел. 6000?
ибо 6050 странно?
дийдроносы хотят Wii пересадить на PX4 diydrones.com/forum/topics/px4-multiwii
а вот колдовство afrowii, MPU6000 + hmc5883
#if defined(MPU6000SPI)
static uint8_t mpuInitialized = 0;
#define MPU_OFF GPIO_WriteHigh(GPIOB, GPIO_PIN_2);
#define MPU_ON GPIO_WriteLow(GPIOB, GPIO_PIN_2);
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_I2C_MST_CTRL 0x24
#define MPUREG_I2C_SLV0_ADDR 0x25
#define MPUREG_I2C_SLV0_REG 0x26
#define MPUREG_I2C_SLV0_CTRL 0x27
#define MPUREG_I2C_SLV4_ADDR 0x31
#define MPUREG_I2C_SLV4_REG 0x32
#define MPUREG_I2C_SLV4_DO 0x33
#define MPUREG_I2C_SLV4_CTRL 0x34
#define MPUREG_I2C_SLV4_DI 0x35
#define MPUREG_I2C_MST_STATUS 0x36
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#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 // Registers 0x49 to 0x60 - External Sensor Data
#define MPUREG_I2C_SLV0_DO 0x63 // This register holds the output data written into Slave 0 when Slave 0 is set to write mode.
#define MPUREG_I2C_MST_DELAY_CTRL 0x67 // I2C Master Delay Control
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
// Configuration bits MPU 6000
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_AFS_2G 0x00
#define BITS_AFS_4G 0x08
#define BITS_AFS_8G 0x10
#define BITS_AFS_16G 0x18
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
#define BITS_DLPF_CFG_42HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define BIT_I2C_SLV0_EN 0x80
static uint8_t MPU6000_Buffer[14 + 6]; // Sensor data ACCXYZ|TEMP|GYROXYZ | Magnetometer data
static uint8_t MPU6000_ReadReg(uint8_t Address)
{
uint8_t rv;
MPU_ON;
spi_writeByte(Address | 0x80); // Address with high bit set = Read operation
rv = spi_readByte();
MPU_OFF;
return rv;
}
static void MPU6000_getSixRawADC(void)
{
uint8_t i;
MPU_ON;
spi_writeByte(MPUREG_ACCEL_XOUT_H | 0x80); // Address with high bit set = Read operation
// ACC X, Y, Z, TEMP, GYRO X, Y, Z, MagData[6]
for (i = 0; i < 14 + 6; i++)
MPU6000_Buffer[i] = spi_readByte();
MPU_OFF;
}
static void MPU6000_WriteReg(uint8_t Address, uint8_t Data)
{
MPU_ON;
spi_writeByte(Address);
spi_writeByte(Data);
MPU_OFF;
delay(1);
}
void MPU6000_init(void)
{
// SPI ChipSelect for MPU-6000
GPIO_Init(GPIOB, GPIO_PIN_2, GPIO_MODE_OUT_PP_HIGH_FAST);
MPU_OFF;
// MPU-6000 input tied to interrupt (TODO). Input-only for now.
GPIO_Init(GPIOB, GPIO_PIN_3, GPIO_MODE_IN_FL_NO_IT);
MPU6000_WriteReg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
delay(100);
MPU6000_WriteReg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); // Set PLL source to gyro output
MPU6000_WriteReg(MPUREG_USER_CTRL, 0b00110000); // I2C_MST_EN
// MPU6000_WriteReg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); // Disable I2C bus
MPU6000_WriteReg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample = 1Khz / (4 + 1) = 200Hz
MPU6000_WriteReg(MPUREG_CONFIG, 0); // BITS_DLPF_CFG_42HZ); // Fs & DLPF Fs = 1kHz, DLPF = 42Hz (low pass filter)
MPU6000_WriteReg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); // Gyro scale 2000?/s
MPU6000_WriteReg(MPUREG_ACCEL_CONFIG, BITS_AFS_4G); // Accel scale 4G
MPU6000_WriteReg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
MPU6000_WriteReg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
mpuInitialized = 1;
}
void ACC_init()
{
if (!mpuInitialized)
MPU6000_init();
acc_1G = 256;
}
void ACC_getADC()
{
MPU6000_getSixRawADC();
ACC_ORIENTATION(-(MPU6000_Buffer[0] << 8 | MPU6000_Buffer[1]) / 16, -(MPU6000_Buffer[2] << 8 | MPU6000_Buffer[3]) / 16, (MPU6000_Buffer[4] << 8 | MPU6000_Buffer[5]) / 16);
ACC_Common();
}
void Gyro_init(void)
{
if (!mpuInitialized)
MPU6000_init();
}
void Gyro_getADC(void)
{
// range: +/- 8192; +/- 2000 deg/sec
MPU6000_getSixRawADC();
GYRO_ORIENTATION((((MPU6000_Buffer[10] << 8) | MPU6000_Buffer[11]) / 4), -(((MPU6000_Buffer[8] << 8) | MPU6000_Buffer[9]) / 4), -(((MPU6000_Buffer[12] << 8) | MPU6000_Buffer[13]) / 4));
GYRO_Common();
}
#define HMC5883L_I2C_ADDRESS 0x1e
#define HMC5883L_ID_REG_A 0x0a
#define HMC5883L_ID_REG_B 0x0b
#define HMC5883L_ID_REG_C 0x0c
#define HMC5883L_MODE_REG 0x02
#define HMC5883L_DATA_OUTPUT_X 0x03
void Mag_init(void)
{
volatile uint8_t i, temp;
// Initialize compass for continous measurement mode
MPU6000_WriteReg(MPUREG_I2C_MST_CTRL, 0b01000000 | 13); // WAIT_FOR_ES=1, I2C Master Clock Speed 400kHz
MPU6000_WriteReg(MPUREG_I2C_SLV4_ADDR, HMC5883L_I2C_ADDRESS); // Write to 5883
MPU6000_WriteReg(MPUREG_I2C_SLV4_REG, HMC5883L_MODE_REG);
MPU6000_WriteReg(MPUREG_I2C_SLV4_DO, 0x00); // Mode register -- value: Continuous-Conversion Mode
MPU6000_WriteReg(MPUREG_I2C_SLV4_CTRL, 0b11000000); // I2C_SLV4_EN | I2C_SLV4_INT_EN
delay(1);
// temp = MPU6000_ReadReg(MPUREG_I2C_MST_STATUS);
// temp = MPU6000_ReadReg(MPUREG_I2C_SLV4_DI);
// Prepare I2C Slave 0 for reading out mag data
MPU6000_WriteReg(MPUREG_I2C_SLV0_ADDR, 0x80 | HMC5883L_I2C_ADDRESS); // Read from 5883
MPU6000_WriteReg(MPUREG_I2C_SLV0_REG, HMC5883L_DATA_OUTPUT_X);
MPU6000_WriteReg(MPUREG_I2C_SLV0_CTRL, 0b10000110); // I2C_SLV0_EN | 6 bytes from mag
delay(1);
}
void Device_Mag_getADC(void)
{
MAG_ORIENTATION( ((MPU6000_Buffer[18] << 8) | MPU6000_Buffer[19]), ((MPU6000_Buffer[14] << 8) | MPU6000_Buffer[15]), ((MPU6000_Buffer[16] << 8) | MPU6000_Buffer[17]) );
}
#endif /* MPU6000SPI */
теперь есть над чем поразмыслить, возможна следующая реинкарнация платы с магнитометром подцепленным к MPU… полный отказ от i2c - барометр по spi, 64-лапый проц(я под него рисовал на бумажке разводку -влазит всё) - следствие (т.к. есть возможность 4 слоя делать) уменьшение размеров? ещё бы где раздобыть драйвер от рук кривых 😃 (программировать научится), ладно надо со своей ещё разобраться, а то пришел час Х - кухню начал делать - как следствие, моё рабочее место кончилось…
полный отказ от i2c - барометр по spi, 64-лапый проц(я под него рисовал на бумажке разводку -влазит всё)
Сергей, разве с одной и2ц не влезет, с чем конфликтует?
ШИМ-входов/выходов 8/8 можно тоже отказаться, достаточно 6/6.
не её можно оставить свободной 😃 но датчики то быстрее читать без неё… я прикидывал 64-х лапый проц, входов -выходов хватает (а можно вход ppm только оставить), два spi, i2c, ацп только по моему один вход остаётся (на той что сейчас v1.2, 1 батарея+4 свободны и выведены)…
не её можно оставить свободной но датчики то быстрее читать без неё…
Дык я только ЗА!
два spi, i2c, ацп только по моему один вход остаётся
Это плохо, лучше было бы 2 вывода оставить (напруга+ток), остальные вообщем и не нужны.
Ты, кстати, родную прошивку через УСБ проверял ?
до усб не добрался, я пока с spi застрял - разбираюсь, воткнул 6000 - в автодетект, вместо 6050, подумал пусть будет - если какой глюк с датчиком до дальше детектирования никуда не уйдёт - аппарат не заведётся 😃
Контроллер должен вываливать вот такое
День убил, - не хочет работать, причем похоже тупо виснет Phiton при выводе графики (с виндой 7 чтоль конфликт)…
В простой версии MiniImu данные с порта бегут в окне “Shell”, но нет изображения 3D кубика.
В версии по Вашей ссылке есть картинка стрелок (статическая) но нет данных в окне “Shell”…
Невезуха …
дийдроносы хотят Wii пересадить на PX4 diydrones.com/forum/topics/px4-multiwii
Уже как то проскакивал порт вия на F4 code.google.com/p/multiwii/source/browse/#svn%2Fbr…
сейчас спрошу глупость: когда читаю, я засылаю что-то типа 0Xff обратно получаю данные, когда записываю - шлю данные - получаю обратно данные -1?..
когда читаю, я засылаю что-то типа 0Xff обратно получаю данные, когда записываю - шлю данные - получаю обратно данные -1?.
Где получаешь? Через printf? Если так, то тег %x даст 0xff, а тег %d даст -1.
2mahowik Александр, прочитал аннотацию к твоей прошивке. Ты пишешь про двойное интегрирование акселя и комплементарный фильтр 99/1. Боковые ускорения тоже как то учитываешь для горизонтальной стабилизации? Как ведёт себя коптер при вибрациях?
Про себя пока сказать нечего, сочиняю более сложную матмодель ИНС (как сказал?!😎), т.к. интегрировать в аксель по простому не получается. Три попытки повисеть на дальномерах дома обернулись царапинами на дверях, жаль не заснял ранее пролёт по коридору. Щас вышел на работу, попробую снова в коридоре.
МПУ приехала, надо теперь запаять окуратно 😃
Ты пишешь про двойное интегрирование акселя и комплементарный фильтр 99/1. Боковые ускорения тоже как то учитываешь для горизонтальной стабилизации? Как ведёт себя коптер при вибрациях?
двойной интегралл по акселю получился стабильным (не без хитростей конечно), но в связке аксель+баро… а по гпс пока не выходит аленький цветочек, либо ошибка где то, либо с коэф. оптимальными не разобрался пока, т.к. мало тестов было из за погоды… если интересуют наработки, пиши в личку…
если интересуют наработки, пиши в личку…
Не буду (пока ) тебе морочить голову в личке, я думаю таких без меня полно:)
Вот хочу сравнить ЛСМку с МПУшкой, у тебя ведь последняя? Можешь дать лог сырых данных акселя с висящего коптера, в этом ведь ничего секретного нет, отсчётов 100?
Как правильно под spi переделать:
ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
if (!ack)
return false;
if (sig != (MPU6050_ADDRESS & 0x7e))
return false;
// determine product ID and accel revision
i2cRead(MPU6050_ADDRESS, 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 {
i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &sig);
rev = sig & 0x0F;
if (!rev) {
failureMode(5);
} else if (rev == 4) {
mpuAccelHalf = 1;
} else {
mpuAccelHalf = 0;
}
- оно надо, ибо это и автодетект, и определение ревизии акселя (потом для настройки нужно)… остался только этот кусочек и можно пробовать с mpu общатся…
Натворил чего-то, завтра даст бог, запаяю мпу, буду пробовать читать…
Не буду (пока ) тебе морочить голову в личке, я думаю таких без меня полно
да без проблем! теребят то в основном по фигне всякой, а тут приятный мужской разговор 😃
Вот хочу сравнить ЛСМку с МПУшкой, у тебя ведь последняя? Можешь дать лог сырых данных акселя с висящего коптера, в этом ведь ничего секретного нет, отсчётов 100?
У меня AIOP 1.0 контроллер с mpu6050 на борту… наверное последняя… а их было несколько версий?
с логами сложнее… я в основном в рантайме на графики смотрю в ГУях, но вроде как андроид прилага EZ-GUI позволяет писать… посмотрю…
а их было несколько версий?
Я имел ввиду что у тебя МПУшка, а не другой датчик.
EZ-GUI позволяет писать… посмотрю…
Хорошо, жду… Если можно побольше отсчётов, а то боюсь для статистики не хватит.
Как правильно под spi переделать
ДОстаточно заменить все функции i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, tmp) аналогичной для СПИ.
тебя МПУшка, а не другой датчик
их несколько ревизий было с разными акселями, а единственное, я так понял, что-бы узнать ревизию надо читать регистры MPU_RA_WHO_AM_I и MPU_RA_PRODUCT_ID, а там ревизий 5+6 в зависимости от модификации какой-то…
Ну вот наконец-то нижняя плата в сборе, не утерпел - мелкоплата лишилась барометра - теперь обратной дороги нет, надеюсь за два-то года хоть что-то стоящее получилось 😃