Создание собственной системы стабилизации

SergDoc

Не ну почему сразу “никак” - оно ж работает - а значит круг поисков уменьшается 😃

oleg70
SergDoc:

Олег, если это не сверхсекретная информация, то можете кусок кода с инициализацией показать…

Код такой же как и у SPI1 и SPI2, которые работают… (просто место в ветке займет),

кстати

SergDoc:

Конфликт с Jtag может быть если SPI3 заведено на порт B

кстати на PORTB у меня SPI1 сидит…
Просто думал может хитрость здесь какая… (у Avr-a была такая фигня) Буду копать…

SergDoc
oleg70:

просто место в ветке займет

а много и не надо, тактирование самого SPI, тактирование портов, настройка портов - это как бы и не много. А так получается ни о чём - не работает и всё, а штатные экстрасенсы уже на новогодние каникулы отправились 😃
Это как я SPI2 в ПО PX4 запускал - в NuttX ошибка была по порту (MOSI или MISO не помню) - всё компилится, а не работает - пока дошло библиотеки перелопатить надо, может и арду на пару месяцев раньше взлетел 😃 ну правда с Александром ещё с УСБ воевали - но это другое…

oleg70
SergDoc:

а много и не надо

// MS5611 Set
//==================================================================
//==================================================================
//==================================================================

/* Configure the chip select pin
in this case we will use PD0 */
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOD, &GPIO_InitStruct);

GPIOD->BSRRL |= GPIO_Pin_0; // high

RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11 | GPIO_Pin_12;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOC, &GPIO_InitStruct);

// connect SPI3 pins to SPI alternate function
GPIO_PinAFConfig(GPIOC, GPIO_PinSource10, GPIO_AF_SPI3);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource11, GPIO_AF_SPI3);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource12, GPIO_AF_SPI3);

// enable peripheral clock

SPI_I2S_DeInit(SPI3);
SPI_InitStruct.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
SPI_InitStruct.SPI_Mode = SPI_Mode_Master;
SPI_InitStruct.SPI_DataSize = SPI_DataSize_8b;
SPI_InitStruct.SPI_CPOL = SPI_CPOL_Low;
SPI_InitStruct.SPI_CPHA = SPI_CPHA_1Edge;
SPI_InitStruct.SPI_NSS = SPI_NSS_Soft | SPI_NSSInternalSoft_Set;
SPI_InitStruct.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4; //40 Mhz / *
SPI_InitStruct.SPI_FirstBit = SPI_FirstBit_MSB;
SPI_Init(SPI3, &SPI_InitStruct);

SPI_Cmd(SPI3, ENABLE); // enable SPI3

SergDoc
SPI_InitStruct.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4; //40 Mhz / * 

А тут правильно?
APB1 и APB2 на разных частотах работают…

APB1 has a max allowed value of 42MHz, APB2 max 84 MHz.

oleg70
SergDoc:

А тут правильно?

Это комментарий просто затесался…
Тут дело такое: я запускаю вот такую “заглушку”
while(1)
{
while (!(SPI1->SR & SPI_SR_TXE));
SPI1->DR = data;
LED_ON
}
в результате на CLK должен быть постоянный сигнал (меандр) + вкл. светодиода (контрольный) , светодиод горит а сигнала нет !, (на мрачные мысли наводит, типа проц./каюк)…

Инициализация вроде правильная…

mahowik
rual:

А ты чем собираешь проект ?

5-й keil

SergDoc:

У Таймкопа нет FPU

ну вот потому и написал что типа зарыл бомбу последователям 😃

rual:

Этаж не важно, оно ж не асме написано, а компиллер сам должен корректно формировать код с ФПУ или без.

все верно… бага в компиллере скорее всего…

SergDoc:

Возможно, глюк в кейле, у меня баро не обрубался 4.7(если не ошибаюсь), а вот удержание высоты, я где-то описывал, ровно 30 сек - потом моторы в минимум - перещёлкиваю тумблер опять висит.

ну так ты словил туже багу 😉 т.е. баро как мы выяснили не обрубался и у меня, а отваливалась переменная интегратора высоты в 1.#QNAN и в гуи оно как ноль видно и в расчетах тоже как ноль берется скорее всего… потому находясь на высоте когда переменную высоты рубило в ноль, моторы и гасли…

А вообще зря я походу взял последнюю версию из гитхаба на переделку… хоть и коП писал что это стабильная версия последняя, выснилось что там даж автоконфигурации nmea модулей нет и mtk1.6-1.9 протоколы не поддерживаются… фильтры дефолтные совсем не для “полететь из короПки”… терь мне понятно почему на naze32 мало народу летает, хотя надо признать есть много удобств по сравнению с вием, типа настройка всего подряд через CLI, т.е. без перезаливки прошивки соот-но…

SergDoc
oleg70:

на мрачные мысли наводит, типа проц./каюк

а порт просто подёргать 1,0…

mahowik

Поправлюсь, для nmea автоконфигурация есть, но кривая. Просто идет авто определение скорости модуля без переключения на нужную, т.е. нашел скажем 38400 бод и все, а то что там 1 герц частота обновления может стоять это уже в садд 😃


case GPS_NMEA:
            // nothing to do, just set baud rate and try receiving some stuff and see if it parses
            serialSetBaudRate(core.gpsport, gpsInitData[gpsData.baudrateIndex].baudrate);
            gpsSetState(GPS_RECEIVINGDATA);
            return;
rual
oleg70:

Инициализация вроде правильная…

Олег, у тебя в тесте СПИ1, а проблемы вроде с СПИ3 были?

mahowik:

все верно… бага в компиллере скорее всего…

Не думаю, кейл НУ очень умный, поэтому при работе с железом надо быть очень внимательным.
Стартап проекта родной кейловский асм? Если да, то замени на этот

startup_stm32f4xx.rar

mahowik
rual:

Не думаю, кейл НУ очень умный, поэтому при работе с железом надо быть очень внимательным.
Стартап проекта родной кейловский асм? Если да, то замени на этот

заменил… не помогло…

в твоем файле ток размер стека и хип поболей и потом если ошибка в общении с FPU то по идее эти параметры не решают проблемы, хотя если основной проц общается с FPU через стек, то может конечно влиять…



Stack_Size      EQU     0x00002000

Heap_Size       EQU     0x00002000
oleg70
rual:

Олег, у тебя в тесте СПИ1,

Мат-перемат…(😃)… Три дня !.. Она зараза такая маленькая, эта единичка…
Все пошло, извиняюсь… (пора перекур сделать с программированием.)

rual
mahowik:

заменил… не помогло…

Мда… Значится что то в коде… Но ты ведь всё равно эти процедуры менять будешь? Меняй, а там посмотришь

oleg70:

Три дня !

Ну так бывает ))) Когда всё само сложное сделано, лажаешь на мелочах, глаз “замыливается”. начинаешь лезть в дебри, а то что под носом не видишь. Свежий взгляд очень помогает…

SergDoc

Да вот платка под MT3329 и эту коробочку
может кому пригодится…
там размер 31 мм оно ложится на нижнюю и входит в верхнюю, плотно закрываясь 😃

SergDoc

Полетал слегка: ветер собачий (при порыве в спину аж толкает) и хоть и GPS протрулюлюкал loc (пищалку на таймере вывел - поёт разные мелодии) и ручки впринципе тянулись к переключателю, но очко не из титана - так в лойтере и не попробовал…
Зато полетал в альтхолд - держит даже в ветер такой очень хорошо 😃
Взлёт и посадка в альтхолд:
Взлёт - моторы крутят на минимуме пока не передвинешь стик больше 50% - сразу резкий отрыв где-то на полметра и всё стик в 50 - висим, больше - подымаемся…
Посадка - немного станновато сделано, возможно из-за сильного ветра, но чтобы посадить надо стик почти в минимум опустить - начинает садиться, надо отдать должное посадка ну оочень мягкая, сам так не посажу (ну разве что в штиль) 😃

Раз альтхолд работает на жестко прикрученной плате, значит аксель себя чувствует хорошо 😃

mahowik
rual:

Мда… Значится что то в коде… Но ты ведь всё равно эти процедуры менять будешь? Меняй, а там посмотришь

В коде вряд ли, т.к. отключение FPU решает проблему. Бага в компиллере явно…
Да и код там простой. Локальная статическая переменная (accAlt) и пару умножений с ней


int getEstimatedAltitude(void)
{
    static uint32_t previousT;
    uint32_t currentT = micros();
    uint32_t dTime;
    int32_t error;
    int32_t baroVel;
    int32_t vel_tmp;
    int32_t BaroAlt_tmp;
    float dt;
    float vel_acc;
    static float vel = 0.0f;
    static float accAlt = 0.0f;
    static int32_t lastBaroAlt;
    static int32_t baroGroundAltitude = 0;
    static int32_t baroGroundPressure = 0;

    dTime = currentT - previousT;
    if (dTime < UPDATE_INTERVAL)
        return 0;
    previousT = currentT;

    if (calibratingB > 0) {
        baroGroundPressure -= baroGroundPressure / 8;
        baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1);
        baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;

        vel = 0;
        accAlt = 0;
        calibratingB--;
    }

    // calculates height from ground via baro readings
    // see: 
    BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (cfg.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
    BaroAlt_tmp -= baroGroundAltitude;
    BaroAlt = lrintf((float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf)); // additional LPF to reduce baro noise

    dt = accTimeSum * 1e-6f; // delta acc reading time in seconds

    // Integrator - velocity, cm/sec
    vel_acc = (float)accSum[2] * accVelScale * (float)accTimeSum / (float)accSumCount;

    // Integrator - Altitude in cm
    accAlt += (vel_acc * 0.5f) * dt  + vel * dt;                                        // integrate velocity to get distance (x= a/2 * t^2)
    accAlt = accAlt * cfg.baro_cf_alt + (float) BaroAlt *(1.0f - cfg.baro_cf_alt);      // complementary filter for Altitude estimation (baro & acc)
    EstAlt = accAlt;
    vel += vel_acc;

#if 0
    debug[0] = accSum[2] / accSumCount; // acceleration
    debug[1] = vel;                     // velocity
    debug[2] = accAlt;                  // height
#endif

    accSum_reset();

    //P
    error = constrain(AltHold - EstAlt, -300, 300);
    error = applyDeadband(error, 10);       // remove small P parametr to reduce noise near zero position
    BaroPID = constrain((cfg.P8[PIDALT] * error / 128), -200, +200);

    //I
    errorAltitudeI += cfg.I8[PIDALT] * error / 64;
    errorAltitudeI = constrain(errorAltitudeI, -50000, 50000);
    BaroPID += errorAltitudeI / 512;     // I in range +/-100

    baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
    lastBaroAlt = BaroAlt;

    baroVel = constrain(baroVel, -300, 300);    // constrain baro velocity +/- 300cm/s
    baroVel = applyDeadband(baroVel, 10);       // to reduce noise near zero

    // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
    // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
    vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);
    vel = constrain(vel, -1000, 1000);                // limit max velocity to +/- 10m/s (36km/h)

    // D
    vel_tmp = lrintf(vel);
    vel_tmp = applyDeadband(vel_tmp, 5);
    vario = vel_tmp;
    BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp / 16, -150, 150);

    return 1;
}
SergDoc:

Взлёт - моторы крутят на минимуме пока не передвинешь стик больше 50% - сразу резкий отрыв где-то на полметра и всё стик в 50 - висим, больше - подымаемся…

я когда единожды ардупират пробовал (в альтхолд), подумал это он так прыгнул из за вибраций на борту, также не исключал, что это они так воздушную подушку побороли 😃
и что в этом хорошего?
Я вес баро просто уменьшал на взлете и отрыв от земли хоть 20см в сек делай, т.е. перевел за 50%, никакого прыжка… взлетаем…
Вот старый видос. Смотреть с 2:40

SergDoc
mahowik:

Бага в компиллере явно…

А они за это ещё и деньги просят 😦

mahowik
SergDoc:

А они за это ещё и деньги просят 😦

Не ну IDE вполне себе мощная, а баги есть везде. В той же Arduino IDE натыкался на пару глюков и пока строчки (независимые от контекста) местами не менял никак не получалось убрать “загадочность”. А пару раз натыкался, когда компиллер не видит закрывающей скобки из-за набора дефайнов внутри, хотя там 100% было все ок… так что это нормально…
И даже в чипах есть баги. И об этом пишут в спеках, что мол при таких то условиях и значениях регистров будет глюк…

upd: По некоторым моим постам можно подумать, что я не люблю арду. Это не так! 😃
Пожалуй самый перспективны проект и мощнейшее комьюнити в тырнете. А вики вообще загляденье!

SergDoc

Я тут вспомнил, помнишь багу по альтхольду в базефлигхт - давно было я ещё на мелкоплате тогда с сонаром ковырялся, а ты делал RC разные, там в ноль переменная становилась всегда из-за неправильного перевода из int в float, я про неё и забыл… может она?

Alexey_1811

Немного не по теме но спрошу тут. Кто то запускал UART4 на STM32F4 (stm32f405rgt6). Ну ни как не выходит передать данные.

//============================== UART_4 FRSKY ==================================
void Init_Uart4(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    USART_InitTypeDef USART_InitStructure;
    /* --------------------------- System Clocks Configuration -----------------*/
  /* UART4 clock enable */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART4, ENABLE);
  /* GPIOA clock enable */
  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
  /*-------------------------- GPIO Configuration ----------------------------*/
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(GPIOA, &GPIO_InitStructure);
  /* Connect USART pins to AF */
  GPIO_PinAFConfig(GPIOA, GPIO_PinSource0, GPIO_AF_UART4);
  GPIO_PinAFConfig(GPIOA, GPIO_PinSource1, GPIO_AF_UART4);

  /* USARTx configuration ------------------------------------------------------*/
  USART_InitStructure.USART_BaudRate = 9600;
  USART_InitStructure.USART_WordLength = USART_WordLength_8b;
  USART_InitStructure.USART_StopBits = USART_StopBits_1;
  USART_InitStructure.USART_Parity = USART_Parity_No;
  USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
  USART_Init(UART4, &USART_InitStructure);
  USART_Cmd(UART4, ENABLE);
}
//==============================================================================
unsigned char _data[10] = {1,2,3,4,5,6,7,8,9,0};
void SendUart4(void)
{
    unsigned char ii;
    for (ii=0;ii<10;ii++)
    {
        while(USART_GetFlagStatus(UART4, USART_FLAG_TXE) == RESET){;} // Wait for Empty
        UART4->DR = (unsigned char)_data[ii];
        __nop();
    //USART_SendData(UART4, data[ii]); // Echo Char
    }
}
SergDoc

возможно надо
USART_SendData(UART4, (uint8_t)_data[ii]);
может его рвёт от char?