Создание собственной системы стабилизации
Я так понял что периферия очень похожа не на F4 а на F10x, ну эт не так страшно, только вот я не усмотрел - а канал какой в DMA или как оно в f4 Stream и Channel ?
Да, Ф3 почти как Ф1. По твоему коду видно что в Ф4 в матрицу ПДП добавлено ещё одно измеренение -Stream.
Что по сути ничего не меняет, в Ф1 просто выбирался канал, а в Ф4 выбирается поток (Stream)с набором каналов (Channel, задается в структуреDMA_InitStructure.DMA_Channel = DMA_Channel_3;) .
Кто нибудь “сырые” данные с LSM303DLHC смотрел ?
Что то у меня он “зверски” шумит (именно аксель, магнетометр приемлемо), буквально данные скачут +/- 16, 32, 64 мл.разр. при любой уст. чувствительности…
Читаю по прерыванию DRDY, причем при некоторых комбинациях datarate/full-scale вообще подвешивает I2C…
Stream
на каждый dma - 8 stream со своими 8-ю каналами в каждом, я там выше табличку выкладывал для dma2 т.к. вся моя периферия висит именно на нём…
буквально данные скачут +/- 16, 32, 64 мл.разр. при любой уст. чувствительности…
ну если вся шкала ±32767 то не так уж и страшно, в вие есть код под 303-ю…
так-же в вие сделано подгоняем 1g =512 и подобран делитель для сырых данных дабы оно соответствовало 512 ещё аксель влючен на 8g- дабы не было переполнения…
у меня сейчас в 330-стоит 400Гц и частота среза 50, но по прикидкам можно наамноого ниже резать… взять что-нибудь такое 50-25 например…
Кто нибудь “сырые” данные с LSM303DLHC смотрел ?
Вот
РусИНС1.0>АП.ИНС.АКСЕЛЬ.ДАННЫЕ:Acc:-0.012,-0.012,1.058 raw:-16,-16,1376
Acc:0.000,0.000,1.058 raw:0,0,1376
Acc:0.025,-0.012,1.071 raw:32,-16,1392
Acc:0.000,0.012,1.034 raw:0,16,1344
Acc:0.000,-0.012,1.083 raw:0,-16,1408
Acc:0.000,-0.049,1.058 raw:0,-64,1376
Acc:0.000,0.000,1.058 raw:0,0,1376
Acc:0.172,0.025,1.034 raw:224,32,1344
Acc:0.000,-0.037,1.046 raw:0,-48,1360
Acc:-0.012,-0.012,1.071 raw:-16,-16,1392
Acc:-0.025,-0.025,1.034 raw:-32,-32,1344
Acc:0.025,0.049,1.132 raw:32,64,1472
Acc:0.012,0.037,1.083 raw:16,48,1408
Acc:-0.012,0.025,1.046 raw:-16,32,1360
Acc:0.012,0.037,1.046 raw:16,48,1360
Acc:-0.012,0.000,1.034 raw:-16,0,1344
Acc:0.025,0.000,1.058 raw:32,0,1376
Acc:0.025,0.000,1.083 raw:32,0,1408
Acc:0.025,0.000,1.058 raw:32,0,1376
Acc:0.012,0.000,1.034 raw:16,0,1344
Acc:0.025,0.000,1.058 raw:32,0,1376
Acc:0.025,-0.025,1.108 raw:32,-32,1440
Acc:-0.025,0.025,1.083 raw:-32,32,1408
Acc:-0.025,0.025,1.058 raw:-32,32,1376
Acc:0.000,-0.012,1.058 raw:0,-16,1376
Acc:0.012,-0.012,1.046 raw:16,-16,1360
Acc:0.025,0.012,1.046 raw:32,16,1360
Acc:-0.025,0.025,1.108 raw:-32,32,1440
Acc:0.000,-0.025,1.034 raw:0,-32,1344
Acc:0.000,0.025,1.058 raw:0,32,1376
Acc:0.000,0.000,1.071 raw:0,0,1392
Acc:-0.012,0.000,1.009 raw:-16,0,1312
Acc:0.012,0.012,1.071 raw:16,16,1392
Acc:0.000,0.000,1.034 raw:0,0,1344
Acc:0.000,0.000,1.034 raw:0,0,1344
Acc:-0.049,-0.025,1.034 raw:-64,-32,1344
Acc:0.025,0.000,1.009 raw:32,0,1312
Acc:0.025,0.000,1.034 raw:32,0,1344
Acc:-0.025,0.025,1.034 raw:-32,32,1344
Acc:-0.012,0.012,1.034 raw:-16,16,1344
Acc:0.000,0.012,1.071 raw:0,16,1392
Acc:0.000,0.000,1.071 raw:0,0,1392
Acc:-0.037,0.025,1.058 raw:-48,32,1376
Acc:0.000,0.025,1.058 raw:0,32,1376
Acc:-0.025,0.000,1.108 raw:-32,0,1440
Acc:0.000,0.000,1.034 raw:0,0,1344
Acc:0.000,0.000,1.083 raw:0,0,1408
Acc:0.025,0.074,1.058 raw:32,96,1376
Acc:0.025,0.012,1.058 raw:32,16,1376
Acc:0.000,0.012,1.058 raw:0,16,1376
Acc:0.025,0.012,1.071 raw:32,16,1392
Acc:-0.012,0.012,1.058 raw:-16,16,1376
Acc:-0.025,0.000,1.058 raw:-32,0,1376
Acc:0.025,0.000,1.034 raw:32,0,1344
Acc:0.000,0.000,1.108 raw:0,0,1440
Acc:0.000,0.000,1.083 raw:0,0,1408
Acc:0.000,-0.012,1.058 raw:0,-16,1376
Acc:-0.012,0.025,1.058 raw:-16,32,1376
Acc:0.000,0.000,1.058 raw:0,0,1376
Acc:0.000,0.000,1.034 raw:0,0,1344
Acc:-0.025,0.012,1.046 raw:-32,16,1360
Acc:-0.012,0.000,1.058 raw:-16,0,1376
Acc:0.000,0.000,1.071 raw:0,0,1392
Acc:0.049,0.000,1.009 raw:64,0,1312
Acc:0.000,0.000,1.009 raw:0,0,1312
Acc:0.000,0.025,1.058 raw:0,32,1376
Acc:-0.025,-0.025,1.058 raw:-32,-32,1376
Что то у меня он “зверски” шумит (именно аксель, магнетометр приемлемо), буквально данные скачут +/- 16, 32, 64 мл.разр. при любой уст. чувствительности…
У меня аксель стоит на 16Г, так что это вполне нормально, шум примерно 2 тысячных
офф-топ, есть проблема!
друг, программер очень хороший!
покатился, то-ли белый-белый совсем горячий, то-ли какая-то из форм шизофрении (как педагог по образованию, изучал и психологию и медицину, сужу) и жена у него - медик, что не видит? дрель свою сегодня забирал у него, жена его помогала ибо он вообще отрешен от деяния сего был, как быть? ну с его женой завтра поговорю - тоже надо осторожно - дела-то не мои, их - семейные, но блин и друг детства - на произвол кидать не хочется…
покатился, то-ли белый-белый совсем горячий, то-ли какая-то из форм шизофрении
“абсолютно здоровых людей нет, есть недообследованные”.
дрель свою сегодня забирал у него, жена его помогала ибо он вообще отрешен от деяния сего был
Проявилось только в этом?
Если да, то может просто устал (весна, авитоминоз, недосып)? Если характер системный, то это кризис наверное, нелады в семье, человек “с тонкой душевной организацией”, “движется по жизни” туда куда не хочет, а куда, где и как свернуть не знает, разочаровался в чем-то важном для него и т.п. Тут “вывод из запоя по фотографии” не возможен, нужно как-то поговорить с человеком. При чём не начинать с распросов и проблем, а организовать (если есть немного времени и денег) выход на природу с шашлыком, на рыбалку, если проблемы не в семье, то семьями, детьми. Главное выдернуть человека из “каши” в которой он “варится”.
Ну а если это
то-ли белый-белый совсем горячий, то-ли какая-то из форм шизофрении
, то только к специалистам.
Осилил “Free IMU” (расчет положения ~12 мк./сек !!! - STM --мооощьь) , только вот данные по углам наблюдаю в терминалке…
Господа подскажите наиболее простой 3D GUI (в этом полный “ноль”), чтоб я мог радоваться дальше, если можно с рецептом установки и связи по COMport-у…
у polulu стырьте програмку для компа и поправьте протокол.
подскажите наиболее простой 3D GUI
у polulu стырьте програмку для компа
Та же прога только дополненная, максимальный протокол,
printf(“!ANG:%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,”,
ToDeg(vI.x),
ToDeg(vI.y),
ToDeg(vI.z),
ToDeg(vU.x),
ToDeg(vU.y),
ToDeg(vU.z),
va.x,va.y,va.z, vm.x,vm.y,vm.z
);
printf(“%.2f,%.2f,%.2f\r\n”,vv.x,vv.y,vv.z);
vI - углы вокруг осей в градусах синий самолётик, vU - углы вокруг осей в градусах красный самолётик, дальше 3 координаты 3х векторов (второй набор углов и вектора можно неуказывать, отбражаться не будут).
Все настроил, но самолетики не движутся, у меня углы в переменных типа “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. Боковые ускорения тоже как то учитываешь для горизонтальной стабилизации? Как ведёт себя коптер при вибрациях?
Про себя пока сказать нечего, сочиняю более сложную матмодель ИНС (как сказал?!😎), т.к. интегрировать в аксель по простому не получается. Три попытки повисеть на дальномерах дома обернулись царапинами на дверях, жаль не заснял ранее пролёт по коридору. Щас вышел на работу, попробую снова в коридоре.