Создание собственной системы стабилизации
Алексей Sir Alex в пирате есть сонар, в какой он стадии, стоит с ним возится (имеется ввиду мелкая плата)…
Сонар я почти год назад довел до ума. Правда не знаю, работает ли он на последних прошивках - не проверял.
Ну поделитесь, если не жалко, повоюю
сорри что чисто кодом:
void incError() {
if(sonarErrors < SONAR_ERROR_MAX) sonarErrors++;
}
void decError(uint8_t limit) {
if(sonarErrors > limit) sonarErrors–;
else sonarErrors = limit;
}
ISR(PORTF_INT0_vect)
{
uint16_t cTime;
static uint16_t edgeTime;
uint8_t pin;
pin = PORTF.IN;
cTime = micros();
sei(); // re-enable interrupts
if(pin & SONAR_READ_MASK) {
edgeTime = cTime;
} else {
#ifdef SONAR_DEBUG
debug[2] = millis() - startTime; // debug measure time to GUI
#endif
sonarData = cTime-edgeTime;
state = 2; // finished measure
//debug[2]=sonarData;
}
}
// Trigger sonar measure and calculate distance
void Sonar_update() {
uint16_t curTime = millis();
uint16_t dTime = curTime - startTime;
// If we are waiting too long, finish waiting and increase error counter
if(dTime > SONAR_MAX_TIME) {
incError();
state = 0;
}
else if(state == 2) { // Measure finished
if(dTime > SONAR_ERROR_TIME) { // wrong time, it should be error!
incError();
state = 0;
} else if(dTime > SONAR_WAIT_TIME) {
uint16_t dist = sonarData/58;
if ((dist < SONAR_MAX_DISTANCE)&(dist > SONAR_MIN_DISTANCE)) { // valid data received
sonarAlt = dist;
decError(0);
} else { incError();}
state = 0; // ready for next measure
}
}
// Start new measure
if(state == 0) {
PORT_SetPins(&PORTF,SONAR_PING);
startTime = curTime;
state = 1;
delayMicroseconds(10);
PORT_ClearPins(&PORTF,SONAR_PING);
}
#ifdef SONAR_DEBUG
debug[0] = sonarAlt;
debug[1] = sonarErrors;
#endif
if (sonarErrors>8) {sonarTrue=0;}
else if (sonarErrors==0) {sonarTrue=1;}
}
итого >8 ошибок = ошибка сонара, снизилось до 0 - все гуд, можно сделать и поменьше
Ну поделитесь, если не жалко, повоюю 😃
я бы порекомендовал драйвер сонара от alexmos… там он довольно грамотно продумал влияние сонара в зависимости от кол-ва ошибок, т.е. переход баро-сонар… я как руки дойдут прикручу его код на новый альт холд…
Погоды нет, даже проверить алт холд, и левел до конца до ума не довёл 😦
Вечером буду пробовать вклинивать сонар 😃
я бы порекомендовал драйвер сонара от alexmos…
а это его большей частью и есть, я ж говорил… сонартруе=0 - переход на баро.
с сонаром пока не справился, пока не смог перенести серву, копаюсь в драйвере сонара
Сергей, какие выводы для эха и тригера ты выбрал? Код предельно прост, в проце void hcsr04_init(sonar_config_t config**)** параметр выбирается из дефайнов какого-то h-файла. Тебе ноужно просто дописать код в секцию switch типа:
case sonar_sergdoc:
trigger_pin = GPIO_Pin_0; // вывод триггера (PB0) - only 3.3v ( add a 1K Ohms resistor )
echo_pin = GPIO_Pin_11; // вывод ЭХА (PB11) - only 3.3v ( add a 1K Ohms resistor )
exti_line = EXTI_Line11;
exti_pin_source = GPIO_PinSource11;
exti_irqn = EXTI11_IRQn;
break;
Нужно ещё найти перечислитель enum с типом sonar_config_t или определитель #define где прописаны конфиги сонара и добавить туда **sonar_**sergdoc, найти вызов функции hcsr04_init в апарметр вставить sonar_sergdoc
Можно вывести сонар и на другой порт, но надо все константы где есть GPIOB переписать.
Ещё момент, этот код для Ф4, для ф1 exti_irqn = EXTI9_5_IRQn; и выбрть вывод эха ТОЛЬКО 0-9 ноги порта.
Ага нашел, почему сонар не включался:
main.c
#ifdef SONAR
// sonar stuff only works with PPM
if (feature(FEATURE_PPM)) {
if (feature(FEATURE_SONAR))
Sonar_init();
}
#endif
осталось моторы сдвинуть…
Я находил где сонар переключается на PB8 - PB9, в коде тупо прописаны
sonar_rc78
вместо него прописать
sonar_pwm56
не помню правда где, но дома посмотрю…
Я находил где сонар переключается на PB8 - PB9, в коде тупо прописаны
Вечером можно списаться, передвинем сонар на любой порт.
лучше мотор с PB8 на PA11 - разъёмы хорошо встают 😃
на этих портах не надо шаманств с резюками…
А вообще можно echo оставить на pb9, а trigger повесить на PA13 или 14 там штырьки есть…
Вот нашел:
sensors.c
void Sonar_init(void)
{
- hcsr04_init(sonar_rc78);
+ hcsr04_init(sonar_pwm56);
sensorsSet(SENSOR_SONAR);
sonarAlt = 0;
}
В общем, поковырялся - сонар в GUI засветился, но на портах PB8, PB9 шимы не выключились, надо вообще их обрубить…
может я ошибаюсь но скорей всего нужно закоментировать всё относящееся к портам PB8 PB9 в драйвере drv_pvm.c , а так же убрать из того же драйвера
if (init->useServos && !init->airplane) {
// remap PWM9+10 as servos (but not in airplane mode LOL)
if (port == PWM9 || port == PWM10)
mask = TYPE_S;
}
port == PWM10 , тогда по идее освободятся порты для сонара и моторы сдвинутся на 1, конечно можно будет использовать только квадрик без подвеса и трикоптер, ну мне большего пока и ненадо от этой платы…
ну мне большего пока и ненадо от этой платы…
Это для маленькой с 103м?
но на портах PB8, PB9 шимы не выключились
надо найти место где инициализация ШИМ выполняется…
да на 103-м
надо найти место где инициализация ШИМ выполняется…
drv_pwm.c - буду ковырять…
drv_pwm.c - буду ковырять…
Найди вызов в майне pwmInit, найди в параметре ссылку на одну из структур
и убери из неё PWM5, PWM6
static const uint8_t multiPWM[] = {
PWM1 | TYPE_IW, // input #1
PWM2 | TYPE_IW,
PWM3 | TYPE_IW,
PWM4 | TYPE_IW,
// PWM5 | TYPE_IW, /// !!!!!!!!!!!!!!!!!!
//PWM6 | TYPE_IW, /// !!!!!!!!!!!!!!!!!!
PWM7 | TYPE_IW,
PWM8 | TYPE_IW, // input #8
PWM9 | TYPE_M, // motor #1 or servo #1 (swap to servo if needed)
PWM10 | TYPE_M, // motor #2 or servo #2 (swap to servo if needed)
PWM11 | TYPE_M, // motor #1 or #3
PWM12 | TYPE_M,
PWM13 | TYPE_M,
PWM14 | TYPE_M, // motor #4 or #6
0xFF
};
или просто закоментируй, ломать код не надо, всё структурами настраивается.
Вот здесь
// this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ]
if (init->airplane)
i = 2; // switch to air hardware config
if (init->usePPM)
i++; // next index is for PPM
setup = hardwareMaps[i];
выбирается одна из структур
static const uint8_t *hardwareMaps[] = {
multiPWM,
multiPPM,
airPWM,
airPPM,
};
которые описаны выше,
static const uint8_t multiPPM[] = {
PWM1 | TYPE_IP, // PPM input
PWM9 | TYPE_M, // Swap to servo if needed
PWM10 | TYPE_M, // Swap to servo if needed
PWM11 | TYPE_M,
PWM12 | TYPE_M,
PWM13 | TYPE_M,
PWM14 | TYPE_M,
PWM5 | TYPE_M, // Swap to servo if needed
PWM6 | TYPE_M, // Swap to servo if needed
PWM7 | TYPE_M, // Swap to servo if needed
PWM8 | TYPE_M, // Swap to servo if needed
0xFF
};
static const uint8_t multiPWM[] = {
PWM1 | TYPE_IW, // input #1
PWM2 | TYPE_IW,
PWM3 | TYPE_IW,
PWM4 | TYPE_IW,
PWM5 | TYPE_IW,
PWM6 | TYPE_IW,
PWM7 | TYPE_IW,
PWM8 | TYPE_IW, // input #8
PWM9 | TYPE_M, // motor #1 or servo #1 (swap to servo if needed)
PWM10 | TYPE_M, // motor #2 or servo #2 (swap to servo if needed)
PWM11 | TYPE_M, // motor #1 or #3
PWM12 | TYPE_M,
PWM13 | TYPE_M,
PWM14 | TYPE_M, // motor #4 or #6
0xFF
};
static const uint8_t airPPM[] = {
PWM1 | TYPE_IP, // PPM input
PWM9 | TYPE_M, // motor #1
PWM10 | TYPE_M, // motor #2
PWM11 | TYPE_S, // servo #1
PWM12 | TYPE_S,
PWM13 | TYPE_S,
PWM14 | TYPE_S, // servo #4
PWM5 | TYPE_S, // servo #5
PWM6 | TYPE_S,
PWM7 | TYPE_S,
PWM8 | TYPE_S, // servo #8
0xFF
};
static const uint8_t airPWM[] = {
PWM1 | TYPE_IW, // input #1
PWM2 | TYPE_IW,
PWM3 | TYPE_IW,
PWM4 | TYPE_IW,
PWM5 | TYPE_IW,
PWM6 | TYPE_IW,
PWM7 | TYPE_IW,
PWM8 | TYPE_IW, // input #8
PWM9 | TYPE_M, // motor #1
PWM10 | TYPE_M, // motor #2
PWM11 | TYPE_S, // servo #1
PWM12 | TYPE_S,
PWM13 | TYPE_S,
PWM14 | TYPE_S, // servo #4
0xFF
};
можешь свою добавить и включить в hardwareMaps[]
Запустил на портах PB8, PB9 проверяю работоспособность остальных шимов…
серва точно работает 😃 моторы не стартонули не один:(
Вобщем надо, свистелку тобиш триггер пересадить на порт pa11
моторы не стартонули не один
ШИМа нет?
как раз то шим есть похоже моторы не назначаются…
Короче я сам дурак - последние прошивки Таймкопа ( там где он якобы отладил алт холд) НЕРАБОЧИЕ - армятся, но шимы не меняются…
откатил прошивку, движки сместились без проблем, но сонар не заработал 😦
Фух всё, работают и движки, и серва, и сонар PB8 - PB9 😃 , а вот по ходу альт холд придётся самому присобачивать, да и всё равно сонар в иму вклинивать надо… пошел я спать…
на картинке я специально сонар вверх вниз дёргал, где скачёк - пальцем закрыл…
Вот только интересный момент, у меня получается на одном таймере теперь висит и 50Гц шим и 400Гц - как-то нехорошо получаеццо…