Создание собственной системы стабилизации
вот такие мы теперь 😃
сделать запуск петли по прерыванию готовности с гир
Огласите (если не секрет) какие общие мысли в этом плане по реализации…
Я тоже сейчас этим занят, пока нет стройной идеи как в реальном времени синхронизировать прерывания от гиры и акселя+маг, “тычу” осциллографом на ноги INT, меняю режимы… (думаю что задачка не из легких, но интересная)
Во всяком случае понятно почему особо ни кто это не делает-, нужно точно высчитывать время запроса/чтения и т.д.
ну вот петля:
currentTime = micros();
if (mcfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + mcfg.looptime;
computeIMU();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime;
#ifdef MPU6000_DMP
mpu6000DmpLoop();
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
int16_t dif = heading - magHold;
if (dif <= -180)
dif += 360;
if (dif >= +180)
dif -= 360;
if (f.SMALL_ANGLES_25)
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg
} else
magHold = heading;
}
#endif
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (f.BARO_MODE) {
static uint8_t isAltHoldChanged = 0;
static int16_t AltHoldCorr = 0;
if (cfg.alt_hold_fast_change) {
// rapid alt changes
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
errorAltitudeI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -cfg.alt_hold_throttle_neutral : cfg.alt_hold_throttle_neutral;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
} else {
// slow alt changes for apfags
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
if (abs(AltHoldCorr) > 500) {
AltHold += AltHoldCorr / 500;
AltHoldCorr %= 500;
}
errorAltitudeI = 0;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = EstAlt;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
}
}
}
#endif
if (sensors(SENSOR_GPS)) {
if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) {
float sin_yaw_y = sinf(heading * 0.0174532925f);
float cos_yaw_x = cosf(heading * 0.0174532925f);
if (cfg.nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -cfg.nav_slew_rate, cfg.nav_slew_rate);
GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
} else {
GPS_angle[ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
GPS_angle[PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
}
}
}
// PID - note this is function pointer set by setPIDController()
pid_controller();
mixTable();
writeServos();
writeMotors();
}
}
в computeIMU(); за одно считываются данные с датчиков - это в imu.c
так вот вместо:
currentTime = micros();
if (mcfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + mcfg.looptime;
сделать запуск по прерыванию от гир, (это я так думаю) ну до этого ещё далеко ибо вожусь в 8-м таймером, не хочет запускаться…
в ардупилоте запуск цикла построен на прерывании от гир, если не ошибаюсь… да и у rual тоже…
но тут сразу поплывут фильтры, и PID надо добивать уже до времени цикла…
Пока имею следующее: прерывания 250 (Гц) =0,004 (Сек) беру с DRDY LSM, при 72 Мгц на борту, в обработчике за половину периода (~0,002 сек.) успеваю три раза прочитать и усреднить гироскоп, прочитать 1 раз акс/маг, выполнить алгоритм ИМУ и даже можно ПИД впихнуть… Вроде все “красиво” и время на другие процессы остается но:
Не нравится - что нет проверки готовности гир при чтении (и похоже что косяки из за этого в данных вылазят)…
DMA к гире подцепил, но та же проблема: идут сбои… как проверить готовность?
готовность только по прерыванию, ибо если читать регистр готовности - пройдёт вечность…
вот петля:
Здесь везде привязка к currentTime = micros();, а мне хочется от нее избавиться
готовность только по прерыванию
Вот здесь то и самое интересное… Например у LSM сигнал DRDY не зависит от установленного DATArate и всегда 250 Гц (?), а L3GD20 просто выставляет “1” на ноге когда можно прочитать данные и ждет пока их не прочитаешь (?)
Тут есть над чем поразмыслить…
чёт опять туплю, это ссылки на адреса памяти?
Да, там расположен уникальный номер проца. Для Ф4 надоть уточнить адрес.
Кста, это при работе УСБ активно используется при создании соединения. Разные дискавери у меня на разные виртуальные компорты садятся.
Например у LSM сигнал DRDY не зависит от установленного DATArate и всегда 250 Гц (?),
Нет, соответствует выставленной частоте отсчётов.
а L3GD20 просто выставляет “1” на ноге когда можно прочитать данные и ждет пока их не прочитаешь (?)
Да, после чтения снимает до очередной готовности (так все СТшные датчики работают и ХМЦ5883 тожа).
Да, там расположен уникальный номер проца. Для Ф4 надоть уточнить адрес.
я уже выковырял 😃 rcopen.com/forum/f134/topic224458/2240
на картинках что выше Unique devise ID в правом нижнем углу 😃
Может кто найдёт ошибку github.com/SergDoc/…/drv_pwm.c
восьмой таймер не запускается, первый такой-же, на нём у меня моторы - работает, а восьмой хоть застрелись…
тактирование таймеров и портов включено отдельно
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_TIM5 | RCC_APB1Periph_I2C2 | RCC_APB1Periph_SPI2 | RCC_APB1Periph_USART2 | RCC_APB1Periph_USART3 , ENABLE);
RCC_APB2PeriphClockCmd( RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM8 | RCC_APB2Periph_ADC1 | RCC_APB2Periph_USART1 | RCC_APB2Periph_SPI1, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOD | RCC_AHB1Periph_GPIOE | RCC_AHB1Periph_DMA2, ENABLE);
RCC_ClearFlag();
затык у меня наверно здесь:
setup = hardwareMaps[i];
for (i = 0; i < MAX_PORTS; i++) {
uint8_t port = setup[i] & 0x0F;
uint8_t mask = setup[i] & 0xF0;
if (setup[i] == 0xFF) // terminator
break;
как этого избежать?
если так?
setup = hardwareMaps[i];
for (i = 0; i < MAX_PORTS; i++) {
uint16_t port = setup[i] & 0x00FF;
uint16_t mask = setup[i] & 0xFF00;
if (setup[i] == 0xFFFF) // terminator
break;
запустил все таймеры 😃
Сегодня поймал глюк приёмника, возможно по этой же причине у меня улетел аппарат в прошлом году, т.к. я заводил 8-й таймер, а он у меня предназначен для серв, естественно надо подключить было серву, а по причине того что питания развязаны и регуль не подключал, то самое простое что придумал, взял питание сервы с приёмника, и вот - АРМ кручу платку - серва крутится - хорошо, ага а дизарм то уже тю-тю - в каком положении стики оставил в том и показания остались - получается приёмник подвис из-за питания, т.к. я ещё и серву подцепил…
приёмник подвис из-за питания
подскажите что за приемник? такой?
Да он самый… но тут получается, что питание от usb через обычный диод т.е. с подключенной сервой ~4.3В получилось, плате то моей не критично у меня всё на 3.3В, а вот приёмнику не понравилось…
так все СТшные датчики работают
Не мог не поделиться…: Решил сделать компенсацию дрейфа “0” гиры от встроенного датчика температуры.
Так вот имеем: при нагреве (t+) “0” гиры смещается в минус и показания датчика температуры тоже (???) уменьшаются…
При комнатной температуре ~24 гр.с. датчик показывает “12” (??) и при нагреве феном уходит в минуса…
Вопрос: это какой то великий замысел или издевательство…(как компенсировать гиру?)
может гдето переполнение?
Вот “матбаза”: Temperature sensor output change vs.temperature -1 °C/digit
Temperature data (1LSB/deg - 8-bit resolution). The value is expressed as two’s complement.
Пишу в коде: int8_t temper=(int8_t)data[…];
Где ошибка?
смотри как в mpu3050 сделано
static void mpu3050ReadTemp(int16_t *tempData)
{
uint8_t buf[2];
i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf);
*tempData = 35 + ((int32_t)(buf[0] << 8 | buf[1]) + 13200) / 280;
}
char
не помогло… (то же самое)
как датчик обзывается?
L3GD20 (от дискавери F3)
буду дома покопаюсь может найду чего…