Создание собственной системы стабилизации
Я все никак c SPI разобраться не могу, может поделитесь кодом и комментариями?
Вот похожее дело, только адреса менять нужно:
// полуаппаратный обмен по SPI допиленный до нужного формата
// если в value приходит 0xaa, то читаем по adr.
// если что либо другое,то пишем это по adr
char SPI(char adr, char value)
{
PORTD &= ~(1<<PD4); // прижимаем CS к земле, давая понять что передача пошла
if (value==0xaa) {adr |= (1<<7);} // ставим флаг чтения
else {adr &= ~(1<<7);} // или записи
USIDR = adr; // заносим в регистр данных USI адрес
TCNT0 = 0x00; // обнуляем таймер
TIMSK |= (1<<TOIE0); // и разрешаем его прерывания
loop_until_bit_is_set(USISR,USIOIF);// ждем передачи байта адреса
USIDR = value; // заносим в регистр данных USI значение
loop_until_bit_is_set(USISR,USIOIF);// ждем передачи байта адреса
TIMSK &= ~(1<<TOIE0); // turn off Timer0 interrupt
PORTD |= (1<<PD4); // отпускаем CS - конец передачи
return USIDR; // возвращаем считанное значение
}
У меня на данный момент по I2C включен (всмысле акселерометр)
интегрирование Рунге-Кутом
У когото в коде встречал данный метод будет время поищу
а тут даже с примером ru.wikipedia.org/wiki/Метод_Рунге_—_Кутты
По моему у меня не проходит колибровка датчиков
//gyros
int inputGyroX[10];//x-axis
float resultGyroX;
int inputGyroY[10];//y-axis
float resultGyroY;
int inputGyroZ[10];//z-axis
float resultGyroZ;
//accelerometers
char inputX[10];//x-axis
float resultX;
char inputY[10];//y-axis
float resultY;
char inputZ[10];//z-axis
float resultZ;
//gyros
int calibrateGyroX()
{
Serial.println(“Calibration Start”);
for(int i=0;i<10;i++)
{
inputGyroX[i] = analogRead(gX);
}
for(int i=0;i<10;i++)
{
resultGyroX += inputGyroX[i];
}
resultGyroX = resultGyroX/10;
return resultGyroX;
Serial.print(resultGyroX);Serial.print(“resultGyroX”);
}
int calibrateGyroY()
{
for(int i=0;i<10;i++)
{
inputGyroY[i] = analogRead(gY);
}
for(int i=0;i<10;i++)
{
resultGyroY += inputGyroY[i];
}
resultGyroY = resultGyroY/10;
return resultGyroY;
}
int calibrateGyroZ()
{
for(int i=0;i<10;i++)
{
inputGyroZ[i] = analogRead(gZ);
}
for(int i=0;i<10;i++)
{
resultGyroZ += inputGyroZ[i];
}
resultGyroZ = resultGyroZ/10;
return resultGyroZ;
}
//accelerometers
int calibrateX()
{
for(int i=0;i<10;i++)
{
Wire.beginTransmission(i2cID);
Wire.send(aX);
Wire.endTransmission();
Wire.requestFrom(i2cID, 1);
if(Wire.available())
{
inputX[i] = Wire.receive();
}
}
for(int i=0;i<10;i++)
{
resultX += inputX[i];
}
resultX = resultX/10;
return resultX;
}
int calibrateY()
{
for(int i=0;i<10;i++)
{
Wire.beginTransmission(i2cID);
Wire.send(aY);
Wire.endTransmission();
Wire.requestFrom(i2cID, 1);
if(Wire.available())
{
inputY[i] = Wire.receive();
}
}
for(int i=0;i<10;i++)
{
resultY += inputY[i];
}
resultY = resultY/10;
return resultY;
}
int calibrateZ()
{
for(int i=0;i<10;i++)
{
Wire.beginTransmission(i2cID);
Wire.send(aZ);
Wire.endTransmission();
Wire.requestFrom(i2cID, 1);
if(Wire.available())
{
inputZ[i] = Wire.receive();
}
}
for(int i=0;i<10;i++)
{
resultZ += inputZ[i];
}
resultZ = resultZ/10;
return resultZ;
Serial.println(“Calibration OK”);
}
Выводит только “Calibration Start”, а дальше молчёк, думаю по сей причине и показания абы что?
Может плюнуть на это дело и написать сразу нулевые значения, ну посчитать их например из 1000 значений лёжа на столе, а потом задефинить и больше их не трогать?
А setup и loop можно глянуть? кстати, если в for одна строчка, {} можно не ставить. Мой коптер уже поднялся в воздух, пока стабилизация только по угловой скорости и настроена не очень.Калибровку можно дефайнить если не боитесь температурных дрифтов байеса, а они будут.
Пока только предположения, я думаю проблема в переменных я суммирую CHAR а потом делю на 10 в десятичной, оно наверно не переваривает этого, надо наверно ввести дополнительную переменную дабы конвертировать это дело в один формат, вот с чем сейчас воюю пока без шим:
ещё проблемка, нормально стабилизацию по курсу без магнитометра никак?
А setup и loop можно глянуть? кстати, если в for одна строчка, {} можно не ставить. Мой коптер уже поднялся в воздух, пока стабилизация только по угловой скорости и настроена не очень.Калибровку можно дефайнить если не боитесь температурных дрифтов байеса, а они будут.
А на ваше программое можно глянуть, или это секрет?
Да с переменной Char не всё так просто, это знак а не число, как его правильно интерпретировать?
Попробуйте делить на 10.0 и использовать int. Курс без магнитометра никак(
используйте int оно от -32768 до 32768, char от -127 до 128
есть еще тип unsigned он знак игнорирует.
используйте int оно от -32768 до 32768, char от -127 до 128
есть еще тип unsigned он знак игнорирует.
прикол в следующем если я буду принимать данные с акселерометра в любую другую переменную не char я тупо теряю знак числа(уже экспериментировал) и тогда получается значение от 0 до 255 а в какую сторону он наклонен бог его знает, я уже писал выше что получалось ускорение свободного падения 55 перевернуть 200 нужно всётаки принять в char, а потом конвертировать как-то?
аксель мне даёт показания в дополнительном коде😢
если в int положить char, то знак потеряться не должен. в процедуре усреднения можно попробовать так:
int middle;
char temp;
middle = 0;
for (c=0; c < сколько циклов усреднения; c++){
temp = процедура чтения акселерометра;
middle = middle + temp;
}
middle = middle / сколько циклов усреднения;
вот так точно будет работать =)
если все совсем плохо, можно использовать float для переменной middle
Вообще-то у меня так и сделано, должна быть какая-то процедура перевода типа этой: atoi в С++
int n;
char *str = “12345”;
n = atoi(str);
только вот работает ли это в ардуине? Вечером попробую…
n = atoi(str)
Тормозить будет по самое небалуйся 😦
Надо не забыть фотку модуля скинуть…
Перечитал еще раз сообщения… Вам надо из CHARа 0…255 сделать signed int -128…+127 и все? Тогда это ж элементарно
char acc;
int res;
res=acc-128;
atoi содержится в стандартном С. в ардуине - х.з. на скорость пока внимание не обращайте. кста, а разве char по умолчанию не signed величина?
Перечитал еще раз сообщения… Вам надо из CHARа 0…255 сделать signed int -128…+127 и все? Тогда это ж элементарно
char acc;
int res;
res=acc-128;
в таком случае и char не надо
int acc;
int res;
res=acc-128;
всё равно больше 255 число я не получу в какую бы переменную я его не загнал
char signed - величина от-128 до +127 если писать Serial.print(X,DEC); оно мне так и выдаёт уже в десятичной -128 +127 но char хранит символы в отличии от byte - где именно цифирки но беззнаковое, я раньше её и пытался приспособить а вот подумать что для получения знака числа отнять от неё 128 недопёр😁
Не не всё так гладко 50 char тоже что и 50 byte, а если теперь отнять 128 получится не 50 и не -50 а ежели платку перевернуть оно покажет в char -50, а в byte 205
atoi не тормозит калибровка пошла, но вот незадача стало ещё всё хуже
Дело в том, что отрицательные числа в ЭВМ хранятся в т.н. дополнительном коде. Он получается из нормального кода инвертированием всех разрядов и добавлением единицы.
Т.е. получается код
int acc;
int res;
if (acc > 127)
res = ~acc + 1; // если реальная разрядность переменной acc не 8 бит, то res = ~(acc - 128) + 1;
else
res = acc;
Хотя при получении отрицательного числа все вышеперечисленное автоматом делается, так что вроде как достаточно самого простого:
if (acc > 127)
res = acc - 128;
else
res = acc;
C char тоже всё работает без всяких извращений, это я лох, полез дальше разбираться где напортачил
timer=millis(); - оно надеюсь берёт время от начала программы?
Ввёл для гироскопов дополнительный фильтр:
#define FI 0.07 - коэффициент
//gyros
int gyroZeroX;//x-axis - откалиброванное значение
float gyroXadc; // отфильтрованное значение показаний
float gyronoXadc; // нефильтрованное значение
float gyroXrate; // реальная я надеюсь угловая скорость
float gyroXangle; // ну и угол
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
gyroXadc=gyroZeroX; // пришлось приравнять перед loop , а то если будет в 0 то полный бред
}
void loop()
{
timer=millis();//start timing
dtime=timer-oldtime;
gyronoXadc = analogRead(gX);
gyroXadc = (gyronoXadc * FI) + (gyroXadc * (1.0 - FI));
gyroXrate = (gyroXadc-gyroZeroX)*2.4; //2.4 это =(Aref/1023)/0.67 0.67-чувствительность гироскопа mV/deg/sec
gyroXangle=gyroXangle+gyroXrate*dtime/1000; // собственно интегрирование
показания стали стабильнее, но углы всёравно плывут, надо чёт придумывать с интегрированием…
Наверно будет лучше поступить так с gyroXrate, а не с gyroXadc ?
Показания и будут плыть.Это абсолютно нормально, супер результат - 1 градус в минуту.Почти не достижимо. Чуть улучшает ситуацию оверсэмплинг, то есть многократное повторное чтение ADC и последующее усреднение значений, можно увеличить таким образом разрешение. Но для нормальной стабилизации по углу нужно делать фьюжн нескольких сенсоров.
И не стоит делать так dtime/1000 , dtime/1000.0 позволит избежать трудновылавливаемых багов.
соскакивает именно первый просчёт угла - время большое, может попробовать так:
если dt>100 то угол=угол (тоесть 0) иначе gyroXangle=gyroXangle+gyroXrate*dtime/1000.0; ?
я просто пока притормозил выполнение перед снятием показаний, время > 600
далее время <20
Ну вот, я опять в тупике, расчёты вроде верные, даже на выходе показания приемлимые, программа выполняется за 20 милисекунд, но вот реакция на повороты в пространстве слишком медленная примерно 1-1.5 секунды, придётся пересмотреть концепцию, боюсь что полностью…