Чтение нескольких каналов PWM от приёмника RC
- Войдите на сайт для отправки комментариев
Добрый день! Хочу попросить помощи у профи. Поправить скетч (если надо) и объяснить некоторые моменты.
Имея весьма примитивное представление о программировании Ардуины, я имел наглость попытаться решить не самую простую задачу. Для управления авиамоделью мне понадобилось преобразование 5 выходных каналов приёмника, с последующей раздачей на 4 сервы. Выходы приёмника - сигнал PWM с частотой 50 Гц. Длина плюсового управляющего импульса 1000 - 2000 мкс.
Главная задача в этой затее - максимально быстро и точно считать выходные сигналы.
Начал я с написания скетча на Pulsein. Он конечно заработал, но столь криво и медленно, что быстро пришло понимание - задачу можно решить только запуском таймеров на прерываниях. Почитал как это сделать для большего чем 2 каналов. Классика - библиотека PinChangeInt.h, альтернатива - библиотека Enableinterrupt.h. Автор и той и другой библиотеки рекомендует второй вариант, а PinChangeInt считает устаревшим.
После тщетных попыток врубиться в суть этих библиотеки, я нашёл готовый шаблон с библиотекой Enableinterrupt для чтения 6 каналов PWM и в меру своего понимания попытался приспособить его под свою задачу. Некоторый прогрес есть - ИДЕ перестала ругаться на мой опус и говорит, что всё ок. Но отдельных местах у меня сомнения.
Скетч для чтения каналов я взял отсюда: http://forum.arduino.cc/index.php?topic=420662.0 Третий по порядку код, пользователя Isaac96.
#include <EnableInterrupt.h>
#define RX_PIN_THROTTLE 2 //input pins
#define RX_PIN_ROLL 3
#define RX_PIN_PITCH 4
#define RX_PIN_YAW 5
#define RX_PIN_AUX1 6
#define RX_PIN_AUX2 7
PROGMEM const byte rxPins[6]={//makes the pinmode easier
RX_PIN_YAW,RX_PIN_ROLL,RX_PIN_PITCH,RX_PIN_AUX1,RX_PIN_AUX2,RX_PIN_THROTTLE};
volatile byte rxState[4]={
0,0,0,0};
volatile int rxPrev[6]={
0,0,0,0,0,0};
void rxInit(){
for(byte i=0;i<6;i++){
pinMode(pgm_read_byte(&rxPins[i]),INPUT);
digitalWrite(pgm_read_byte(&rxPins[i]),HIGH);
}
enableInterrupt(RX_PIN_THROTTLE,rxGoesHigh1,RISING);
enableInterrupt(RX_PIN_ROLL,rxGoesHigh2,RISING);
enableInterrupt(RX_PIN_PITCH,rxGoesHigh3,RISING);
enableInterrupt(RX_PIN_YAW,rxGoesHigh4,RISING);
enableInterrupt(RX_PIN_AUX1,rxGoesHigh5,RISING);
enableInterrupt(RX_PIN_AUX2,rxGoesHigh6,RISING);
}
void rxGoesHigh1(){
enableInterrupt(RX_PIN_THRoTTLE,rxGoesLow1,FALLING);
rxPrev[0]=micros();
}
void rxGoesLow1(){
enableInterrupt(RX_PIN_THROTTLE,rxGoesHigh1,RISING);
rxVal[0]=micros()-rxPrev[0];
}
void rxGoesHigh2(){
enableInterrupt(RX_PIN_ROLL,rxGoesLow2,FALLING);
rxPrev[1]=micros();
}
void rxGoesLow2(){
enableInterrupt(RX_PIN_ROLL,rxGoesHigh2,RISING);
rxVal[1]=micros()-rxPrev[1];
}
void rxGoesHigh3(){
enableInterrupt(RX_PIN_PITCH,rxGoesLow3,FALLING);
rxPrev[2]=micros();
}
void rxGoesLow3(){
enableInterrupt(RX_PIN_PITCH,rxGoesHigh3,RISING);
rxVal[2]=micros()-rxPrev[2];
}
void rxGoesHigh4(){
enableInterrupt(RX_PIN_YAW,rxGoesLow4,FALLING);
rxPrev[3]=micros();
}
void rxGoesLow4(){
enableInterrupt(RX_PIN_YAW,rxGoesHigh4,RISING);
rxVal[3]=micros()-rxPrev[3];
}
void rxGoesHigh5(){
enableInterrupt(RX_PIN_AUX1,rxGoesLow5,FALLING);
rxPrev[4]=micros();
}
void rxGoesLow5(){
enableInterrupt(RX_PIN_AUX1,rxGoesHigh5,RISING);
rxVal[4]=micros()-rxPrev[4];
}
void rxGoesHigh6(){
enableInterrupt(RX_PIN_AUX2,rxGoesLow6,FALLING);
rxPrev[5]=micros();
}
void rxGoesLow6(){
enableInterrupt(RX_PIN_AUX2,rxGoesHigh6,RISING);
rxVal[5]=micros()-rxPrev[5];
}
Этот код - шаблон, там нет основного цикла и настроек. Если их добавить, хоть с пустыми скобками, то ИДЕ начинает искать ошибки. Первая грамматическая, название переменной. Вторая - не назначены переменные rxVal[0-5], а вот тут я в сомнениях. Автор написал странную фразу: "The values are storeed in rxVal[0-5] in order TAER12" Что есть последовательностьTAER12, я тнигде на нашёл. Наверное, жаргон. В своём скетче я тупо пронумеровал эти переменные от нуля до 4-х (у меня на один канал меньше).
Второе что я правил без малейшего понимания, это последовательность назначения пинов в строках:
PROGMEM const byte rxPins[6]={//makes the pinmode easier
RX_PIN_YAW,RX_PIN_ROLL,RX_PIN_PITCH,RX_PIN_AUX1,RX_PIN_AUX2,RX_PIN_THROTTLE};
Последовательность довольно странная и отличается от последовательности задания имён константам. Почему так? Это важно?
В итоге, скетч я написал такой:
Автор написал странную фразу: "The values are storeed in rxVal[0-5] in order TAER12" Что есть последовательностьTAER12, я тнигде на нашёл. Наверное, жаргон.
Это не жаргон, а обозначение каналов, в данном случае их 6шт(0-5) а ТАЕR12 это последовательность их расшивровки - THR, AIL, ELEV, RUDD, AUX1, AUX2 - газ, элерон, елеватор(РВ), руддер(РН) и два канала управления(режимы или доп каналы).
Можно вопрос- к чему этот проэкт, что будет управлятся? Может взять уже куски готового кода, правда это универсальная платформа автопилота, но там все это есть, можно надергать кусков, которые надо.
https://github.com/multiwii/multiwii-firmware
Можно вопрос- к чему этот проэкт, что будет управлятся? Может взять уже куски готового кода, правда это универсальная платформа автопилота, но там все это есть, можно надергать кусков, которые надо.
https://github.com/multiwii/multiwii-firmware
Потрясающе! Не допёр :) (это про ТАЕR12!)
Проект довольно завиральный. Эдакий "недоконвертоплан" : http://forum.rcdesign.ru/f90/thread483403.html
За пару дней я немерянно продвинулся, но цели пока не достиг. В примере с которого я много слизал есть явные ошибки, такие как переменные нигде не используемые. В итоге, состряпал нечто похожее. Последняя версия скетча в конце. Он работает, но крайне ненадёжно. С одним каналом - чисто шоколад. Но даже два канала одновременно приводят к быстрому сбою. Сериал начинает печатать 4-х значные числа. Что интересно: эти непонятные цифири зависят от входных сигналов, т.е. меняются при шевелении стиками.
Чтобы надёргать из готового, придётся его прочитать и понять. Не мой уровень, увы. Да и надо-то всего-навсего прочитать каналы.
Вы наверное скажете, что уж этого добра в готовом виде - завались. Увы, это не так. Я надёргал под десяток готовых кодолв для чиения нескольких каналов PWM, и ни один из них не работает! Что особенно убийственно, даже с официального сайта arduino.cc. Их вариант по ссылке: http://playground.arduino.cc/Code/ReadReceiver У меня их код даёт ошибку "error: within this context". Вот и догадайся, в котексте чего там должно быть :)
//#include <Servo.h> #include <EnableInterrupt.h> volatile int start[]={1,2,3,4,5}; volatile int imp[]={1,2,3,4,5}; //Servo mLeft;//выход левый мотор //Servo mRight;//выход правый мотор //Servo throtle;//выход газ //Servo rotate;//серва поворота траверсы void setup() { pinMode(4,INPUT); pinMode(5,INPUT); pinMode(6,INPUT); pinMode(7,INPUT); pinMode(8,INPUT); digitalWrite(4,LOW); digitalWrite(5,LOW); digitalWrite(6,LOW); digitalWrite(7,LOW); digitalWrite(8,LOW); /*mLeft.attach(10); //привязываем левый мотор к порту 10 mRight.attach(11); //привязываем правый мотор к порту 11 throtle.attach(9); //Маршевый мотор выход пин 9 rotate.attach(3); //серва поворота траверсы пин 3 mLeft.writeMicroseconds(1000); //ставим все сервы на ноль mRight.writeMicroseconds(1000); throtle.writeMicroseconds(1500); rotate.writeMicroseconds(1000);*/ Serial.begin(9600); enableInterrupt(4,High1,RISING);//управление режимом enableInterrupt(5,High2,RISING);//крен enableInterrupt(6,High3,RISING);//левый стабилизатор enableInterrupt(7,High4,RISING);//газ enableInterrupt(8,High5,RISING);//Правый стабилизатор } void loop() { Serial.println("------------------------------"); Serial.print(" ch1 : "); Serial.print(imp[1]); Serial.print(" ch2 : "); Serial.print(imp[2]); Serial.print(" ch3 : "); Serial.print(imp[3]); Serial.print(" ch4 : "); Serial.print(imp[4]); Serial.print(" ch5 : "); Serial.print(imp[5]); Serial.println(); /* mLeft.writeMicroseconds(imp[1]); // mRight.writeMicroseconds(imp[2]); // throtle.writeMicroseconds(imp[3]); // rotate.writeMicroseconds(imp[4]);*/ } void High1(){ enableInterrupt(4,Low1,FALLING); start[1]=micros(); } void Low1(){ enableInterrupt(4,High1,RISING); imp[1]=micros()-start[1]; } void High2(){ enableInterrupt(5,Low2,FALLING); start[2]=micros(); } void Low2(){ enableInterrupt(5,High2,RISING); imp[2]=micros()-start[2]; } void High3(){ enableInterrupt(6,Low3,FALLING); start[3]=micros(); } void Low3(){ enableInterrupt(6,High3,RISING); imp[3]=micros()-start[3]; } void High4(){ enableInterrupt(7,Low4,FALLING); start[4]=micros(); } void Low4(){ enableInterrupt(7,High4,RISING); imp[4]=micros()-start[4]; } void High5(){ enableInterrupt(8,Low5,FALLING); start[5]=micros(); } void Low5(){ enableInterrupt(8,High5,RISING); imp[5]=micros()-start[5]; }Илья МГУ, у вас сама идеология захвата шим не позволит работать без глюков. Нужно каждому входящему сигналу отдельное физическое прерывание, и таковых есть аж 6 в меге328. Два INT, три групповых PCINT и одно прерывание компаратора. Поскольку самостоятельно вы бы не написали вот вам шапка, в прерывания вставляете захват микросов, или что там вам нужно:
void setup() { pinMode(2,INPUT_PULLUP); //in1 pinMode(3,INPUT_PULLUP); //in2 pinMode(8,INPUT_PULLUP); //in3 pinMode(A0,INPUT_PULLUP); //in4 pinMode(4,INPUT_PULLUP); //in5 EICRA|= (1<<ISC10) | (1<<ISC10); EIMSK|= (1<<INT1) | (1<<INT0); PCICR|= (1<<PCIE2) |(1<<PCIE1) |(1<<PCIE0); PCMSK0|= 1<<PCINT0; PCMSK1|= 1<<PCINT8; PCMSK2|= 1<<PCINT20; } void loop() { } ISR (INT0_vect) { } ISR (INT1_vect) { } ISR (PCINT0_vect) { } ISR (PCINT1_vect) { } ISR (PCINT2_vect) { }вот вам шапка, в прерывания вставляете захват микросов, или что там вам нужно:
Спасибо огромное!
Ещё вопрос, по обработчику прерываний. Похоже классика выглядит так:
void calcThrottlePulse() { if(PCintPort::pinState) { unThrottleInStart = TCNT1; } else { unThrottleInShared = (TCNT1 - unThrottleInStart)>>1; bUpdateFlagsShared |= THROTTLE_FLAG; } }Не зря ли я повёлся на сомнительную альтернативу, в виде двух функций и двух прерываний на каждый канал? Или, этот метод тоже работоспособен?
Илья МГУ, не знаю откуда вы выдрали этот кусок. Судя по всему он считает длительность в тактах таймера1. Я не считаю нужным привлекать к счёту времени таймер1, т.к. это потянет за собой новые проблемы.
Илья МГУ, не знаю откуда вы выдрали этот кусок.
Кусок отсюда: http://rcarduino.blogspot.ru/2012/11/how-to-read-rc-channels-rcarduinofastlib.html
Называется "Updated ISR" Чуть выше ещё один вариант.
Произошло невероятное. Мой скетч из поста №2 работает. Причём, работает отлично! Без сбоев и глюков. Проблема была в другом, а именно в характере принимаемых PWM сигналов. Об этом тут:
http://arduino.ru/forum/apparatnye-voprosy/vopros-po-signalam-pwm-dlya-servomashinok-arduino-biblioteka-servo#comment-271502
Илья МГУ, я вам об этом в #3 и писал, -одновременно все каналы тот вариант не может обслужить по принципиальным причинам. Вы читаете, или я пишу в пустоту?
Читаю, не сомневайтесь. На мысль об одновременности каналов автопилота навёл именно удачный опыт с приёмником и кодом из поста №2. Теперь понятно, что он работает только со сдвинутыми по времени импульсами. А мне надо считывать импульсы, которые накладываются друг на друга.
Ваш вариант позволит считать импульсы одновременно? Я не очень понимаю, как в этом случае огранизовать прерывания для считывания импульсов. Они ведь тоже должны осуществляться одновременно? Кая я всегда считал, Ардуино всё выполняет последовательно. Это не так?
Я не очень понимаю, как в этом случае огранизовать прерывания для считывания импульсов. Они ведь тоже должны осуществляться одновременно? Кая я всегда считал, Ардуино всё выполняет последовательно. Это не так?
Всё уже организовано, поднимите очи вверх. Вам осталось лишь собрать данные внутри прерываний. Ардуина обрабатывает последовательно, но в вашем коде, в случае прихода двух и более сигналов гарантированно обрабатывается только первый, а в моём все, но вектора прерываний выполнятся по-очереди.
dimax, в Вашем коде мне были знакомы отдельные слова типа void loop :) Сам бы ни в жисть не разобрался. Помог программист из ближайшего окружения. Получился такой код:
#include <Servo.h> int pitch; int roll; int ThrCorrLeft; int ThrCorrRight; int ThrOut; Servo mLeft;//выход левый мотор Servo mRight;//выход правый мотор Servo throtle;//выход газ // Время обнаружения возрастающего фронта, мс volatile int rxPrev[5]={0,0,0,0,0}; // Номера элементов в массиве соответствуют пинам: {INT0, INT1, PCINT0, PCINT8, PCINT20} // Текущее значение длины импульса, мс volatile int rxVal[5]={0,0,0,0,0}; // Номера элементов в массиве соответствуют пинам: {INT0, INT1, PCINT0, PCINT8, PCINT20} void setup() { pinMode(2,INPUT_PULLUP); // Настройка пина с номером PCINT18/INT0 на приём с подтягивающим резистором pinMode(3,INPUT_PULLUP); // Настройка пина с номером PCINT19/INT1 на приём с подтягивающим резистором pinMode(4,INPUT_PULLUP); // Настройка пина с номером PCINT20 на приём с подтягивающим резистором pinMode(8,INPUT_PULLUP); // Настройка пина с номером PCINT0 на приём с подтягивающим резистором pinMode(A0,INPUT_PULLUP); // Настройка пина с номером PCINT8 на приём с подтягивающим резистором // См. схему размещения пинов https://www.arduino.cc/en/Hacking/Atmega168Hardware // Настраивает обработку прерываний на пинах INT1 и INT2 (INT0_vect и INT1_vect) на срабатывание при падении или возрастании фронта EICRA|= (1<<ISC10) | (1<<ISC00); // тут была ошибка // Разрешает обработку прерываний на пинах INT1 и INT0 EIMSK|= (1<<INT1) | (1<<INT0); // Устанавливает прерывания на пинах: // Диапазон пинов PCINT[23..16] (PCIE2) // Диапазон пинов PCINT[14..8] (PCIE1) // Диапазон пинов PCINT[7..0] (PCIE0) PCICR|= (1<<PCIE2) |(1<<PCIE1) |(1<<PCIE0); // После этого: // ЛЮБОЕ изменение сигнала на любом из пинов PCINT23..16 будет вызывать физическое прерывание PCINT2_vect // ЛЮБОЕ изменение сигнала на любом из пинов PCINT14..8 будет вызывать физическое прерывание PCINT1_vect // ЛЮБОЕ изменение сигнала на любом из пинов PCINT7..0 будет вызывать физическое прерывание PCINT0_vect // Настраивает прерывание PCINT0_vect так, чтобы оно реагировало только на изменение сигнала на пине PCINT0 PCMSK0|= 1<<PCINT0; // Настраивает прерывание PCINT1_vect так, чтобы оно реагировало только на изменение сигнала на пине PCINT8 PCMSK1|= 1<<PCINT8; // Настраивает прерывание PCINT2_vect так, чтобы оно реагировало только на изменение сигнала на пине PCINT20 PCMSK2|= 1<<PCINT20; mLeft.attach(10,1000,2000); //привязываем левый мотор к порту 10 mRight.attach(11,1000,2000); //привязываем правый мотор к порту 11 throtle.attach(9); //Маршевый мотор выход пин 9 mLeft.writeMicroseconds(1000); //ставим все сервы на ноль mRight.writeMicroseconds(1000); throtle.writeMicroseconds(1500); Serial.begin(9600); } void loop() { if(1465<=rxVal[0]){ //motors corr start pitch=rxVal[2]-rxVal[4]; roll=rxVal[1]-1500; ThrCorrLeft=pitch*8+1000-roll;//здесь менять коэффициенты ThrCorrRight=pitch*8+1000+roll;//здесь менять коэффициенты } else { if(rxVal[0]<1115){ ThrCorrLeft=rxVal[3]; ThrCorrRight=rxVal[3]; } else { ThrCorrLeft=1000; ThrCorrRight=1000; }} //motors corr fin if(rxVal[0]<1775){ //throtle start ThrOut=rxVal[3]/2+1000; } else { ThrOut=2000-rxVal[3]/2; } //throtle fin mLeft.writeMicroseconds(ThrCorrLeft); mRight.writeMicroseconds(ThrCorrRight); throtle.writeMicroseconds(ThrOut); Serial.println("-------------------------------------------------"); Serial.print(" ch7: "); Serial.print(rxVal[0]); Serial.print(" aierons: "); Serial.print(rxVal[1]); Serial.print(" Stab_Left: "); Serial.print(rxVal[2]); Serial.print(" Throtle: "); Serial.print(rxVal[3]); Serial.print(" Stab_Right: "); Serial.print(rxVal[4]); Serial.print(" roll: "); Serial.print(roll); Serial.print(" !Difference: "); Serial.print(pitch); Serial.print(" Left: "); Serial.print(ThrCorrLeft); Serial.print(" Right: "); Serial.print(ThrCorrRight); Serial.print(" Throtle: "); Serial.print(ThrOut); Serial.println(); } // Обработчик прерывания на пине INT0 ISR (INT0_vect) { if(digitalRead(2)==HIGH) { // Обнаружен возрастающий фронт на INT0, запоминаем значение таймера rxPrev[0] = micros(); } else { // Обнаружен падающий фронт на INT0, вычисляем значение шим rxVal[0] = micros() - rxPrev[0]; } } // Обработчик прерывания на пине INT1 ISR (INT1_vect) { if(digitalRead(3)==HIGH) { // Обнаружен возрастающий фронт на INT1, запоминаем значение таймера rxPrev[1] = micros(); } else { // Обнаружен падающий фронт на INT1, вычисляем значение шим rxVal[1] = micros() - rxPrev[1]; } } // Обработчик прерывания на пине PCINT0 ISR (PCINT0_vect) { if(digitalRead(8)==HIGH) { // Обнаружен возрастающий фронт на PCINT0, запоминаем значение таймера rxPrev[2] = micros(); } else { // Обнаружен падающий фронт на PCINT0, вычисляем значение шим rxVal[2] = micros() - rxPrev[2]; } } // Обработчик прерывания на пине PCINT8 ISR (PCINT1_vect) { if(digitalRead(A0)==HIGH) { // Обнаружен возрастающий фронт на PCINT8, запоминаем значение таймера rxPrev[3] = micros(); } else { // Обнаружен падающий фронт на PCINT8, вычисляем значение шим rxVal[3] = micros() - rxPrev[3]; } } // Обработчик прерывания на пине PCINT20 ISR (PCINT2_vect) { if(digitalRead(4)==HIGH) { // Обнаружен возрастающий фронт на PCINT20, запоминаем значение таймера rxPrev[4] = micros(); } else { // Обнаружен падающий фронт на PCINT20, вычисляем значение шим rxVal[4] = micros() - rxPrev[4]; } } // Полная документация на ATMega 328, где описаны все регистры и константы: // http://www.atmel.com/ru/ru/Images/Atmel-42735-8-bit-AVR-Microcontroller-ATmega328-328P_Datasheet.pdfГляньте (если не лень), всё ли правильно понято.
ISR (PCINT1_vect) { if(digitalRead(A0)==HIGH) { ....Эта конструкция очень не устойчивая. Если прерывание срабатывает на фронт, то надо вводить небольшую задержку несколько микросекунд с помощью цикла FOR, иначе будут ложные срабатывания при дребезге на фронте/спаде.
Спасибо!
Но откуда дребезг? К пинам подключаются цифровые выходы с автопилота. Никаких кнопок.