Специальные цены   новые товары
+ Ответить в теме
Страница 153 из 165 ПерваяПервая ... 143 151 152 153 154 155 163 ... ПоследняяПоследняя
Показано с 6,081 по 6,120 из 6569

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

Тема раздела Квадрокоптеры. Общие вопросы в категории Квадрокоптеры и мультироторы; Сообщение от rual FlexyPort->Telemetry, сохраняю, но не работает зараза х.з. может он вообще нерабочий, я хотел его проверить, но руки ...

  1. #6081

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Цитата Сообщение от rual Посмотреть сообщение
    FlexyPort->Telemetry, сохраняю, но не работает зараза
    х.з. может он вообще нерабочий, я хотел его проверить, но руки до него не дошли, там ещё косяков на параход, покатай опенпилот: https://github.com/openpilot/OpenPilot , HAL почти тот же самый.... на flexi в опенпилоте я вешал gps - работал...
    тут HAL http://forum.rcdesign.ru/blogs/74247/blog19886.html
    Цитата Сообщение от alexeykozin Посмотреть сообщение
    Махови проверял задержку времени получаемую от медиатека 3333 в сопоставлении с атомными часами, он утверждает что время почти не запаздывает, но это не говорит о том что не запаздывают координаты или курс
    Алексей чем отличается протокол в 3329 и 3333 - беда в том что 3329 ловит 6 спутников но фикс и hdop есть, а 3333 - показывает у меня координаты и 12 спутников дома, но нет ни фикса, ни hdop, ни vdop нету (
    Последний раз редактировалось SergDoc; 23.12.2015 в 10:49.

  2.  
  3. #6082

    Регистрация
    11.05.2008
    Адрес
    Великий Новгород
    Возраст
    38
    Сообщений
    3,939
    Записей в дневнике
    22
    Цитата Сообщение от alexmos Посмотреть сообщение
    Гироскоп нужно корректировать всегда, и даже если коррекция будет с плавающим весом, угадать когда понижать вес, гарантированно точно нельзя.
    в приснопамятном CC когда-то существовала ветка с банальной коррекцией в пару строк: чем ближе длина вектора акселя к единице (ну или к нулю, после вычета G), тем больше его вес. и наоборот, чем больше текущее ускорение, тем меньше он "корректирует" гироскопы. если в полете наблюдалось совсем уж неприятное уплывание, то самолет достаточно было вывести в прямолинейный полет на пару секунд, а коптер ручками завесить.

    дополнительно пытались ввести коррекцию по смещению акселя от центра масс аппарата (т. е. из акселя вычитались угловые ускорения). для полного счастья нужны были пара датчиков симметрично равноудаленных от ЦМ, но все уперлось в быстродействие. или в появление CC3D, не помню уже

  4. #6083

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Цитата Сообщение от HikeR Посмотреть сообщение
    тем больше его вес. и наоборот, чем больше текущее ускорение, тем меньше он "корректирует"
    там оно есть, но как-то странно - я не понял, там сделан ПИД-регулятор веса акселя, плюс тоже что и в арду - при арме вес акселя в 10 раз уменьшается...

  5. #6084

    Регистрация
    26.11.2012
    Адрес
    Tambov
    Возраст
    46
    Сообщений
    777
    Цитата Сообщение от SergDoc Посмотреть сообщение
    - при арме вес акселя в 10 раз уменьшается...
    наверно это как раз включается "полетный рабочий вес", а на земле для ускорения процесса выравнивания горизонта вес завышенный.... что логично.

  6.  
  7. #6085

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Цитата Сообщение от rual Посмотреть сообщение
    в настройках платы выбираю FlexyPort->Telemetry
    тут подумалось - сделай HAL из AQ32 борды, там вроде порты обозваны не майн и флекси, а усарт такой-то и их там больше...

  8. #6086

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    тут подумалось - сделай HAL из AQ32 борды, там вроде порты обозваны не майн и флекси, а усарт такой-то и их там больше...
    Серег у меня плата уже прошитая, Sparky на Ф3м, переделывать особо не зачем, я её брал под свое ПО. Просто хотел посмотреть какой там алго в плане реакции на боковые ускорения. Было бы интересно последнюю калманутую прошивку пиксхавка тоже покатать.

  9. #6087

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    просто я думал ты делал на ф4бы, а aq32 похожа ну порты переписать только, я собирался хал сделать, но сейчас есть другие задачи...

  10.  
  11. #6088

    Регистрация
    26.11.2012
    Адрес
    Tambov
    Возраст
    46
    Сообщений
    777
    Цитата Сообщение от rual Посмотреть сообщение
    Пиши, спрашивай, что смогу отвечу.
    Александр, у меня вопрос: как в питоне можно <структуру> данных создать, аналогичную СИ ??
    Замысел такой: передавать по указателю массив байт, по SPI, из структуры STM (Си) в аналогичную структуру RPi под питоном..
    Короче - реализовать синхронизацию разнотипных переменных за одну транзакцию (?)..

  12. #6089

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    у нас событие..
    первый красивый полет на ардукоптере с полностью своей инерциалкой
    ссылка
    полет тимура

  13. #6090

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от alexeykozin Посмотреть сообщение
    у нас событие..
    Отлично! Поздравляю!
    Есть вопросы, я так понял что угол искривляли на +-5 гр, не мало? Какова длительность периода адаптации? При адаптации крутите угол яв?

  14. #6091

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    сделали несколько вещей
    - убрали имакс в пидах лоитера до минимума
    - добавили лоитер D у ардукоптера он был 0
    - добавил компенсацию времени задержки координат жпсом, причем в отличие от кода ардукоптера сделал ее регулируемой

    при этом при существенных ошибках компаса исчезла склонность начать нарезать спирали

  15. #6092

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от alexeykozin Посмотреть сообщение
    - убрали имакс в пидах лоитера до минимума
    это верно, в части уменьшения расскачки, но нужно проверить реакцию на боковой ветер, может не хватить "подтяга" против ветра

    Цитата Сообщение от alexeykozin Посмотреть сообщение
    добавили лоитер D у ардукоптера он был 0
    это хорошо, и если Д берется от инерциалки (т.е. это боковые ускорения с акселя), т.е. НЕ ЗАВИСИТ от ориентации азимута, это основной момент в борьбе с унитазом
    Цитата Сообщение от alexeykozin Посмотреть сообщение
    - добавил компенсацию времени задержки координат жпсом, причем в отличие от кода ардукоптера сделал ее регулируемой
    изменяемая позиция выборки из буфера инециалки?

  16. #6093

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    Цитата Сообщение от rual Посмотреть сообщение
    изменяемая позиция выборки из буфера инециалки?
    жпс настраивается на 10 гц
    при каждом поступлении новых данных запоминается текущие значения позиции и скорости инерциалки и проталкивается вниз буффера
    буффер на 5 ячеек, в зависимости от необходимости можно снимать задержку за 1,2,3 или 4 сэмпла назад тоесть за 0,1 - 0,4 сек
    вычитая из текущего состояния инерциалки ее состояние некоторое время назад получаем дельту позиции и скорости за заданный промежуток времени
    эту дельту добавляем к текущим показаниям жпс при выборе точки к которой "подтягивается инерциалка"

  17. #6094

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от alexeykozin Посмотреть сообщение
    вычитая из текущего состояния инерциалки ее состояние некоторое время назад получаем дельту позиции и скорости за заданный промежуток времени
    эту дельту добавляем к текущим показаниям жпс при выборе точки к которой "подтягивается инерциалка"
    Мудрёно))

  18. #6095

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Алексей, терзают смутные сомнения, в твоей коррекции присутствует положительная обратная связь, ГНСС+дельтаИНС тем больше, чем больше убегание ИНС вперед за время задержки от предыдущего состояния. Я всё правильно понял, не ошибся? Если так, то чревато расскачкой (причем внутренней, математической) даже с ровным наложением ИНС и азимута.

  19. #6096

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    Цитата Сообщение от rual Посмотреть сообщение
    ГНСС+дельтаИНС тем больше, чем больше убегание ИНС вперед за время задержки от предыдущего состояния. Я всё правильно понял, не ошибся?
    это сложно назвать положительной обратной связью по одной простой причине. дельта инс не имеет интегратора, она всегда берет краткосрочные данные инерциалки. собственно в этом и фенечка. Инс особенно с нашими сенсорами и частотой обработки штука очень быстро деградирующая во времени, но в краткосрочном промежутке она очень даже неплоха. к ГНСС добавляется очень краткосрочный прогноз

    эквивалентом было бы сохранить в массив на 50-100 элементов данных в быстром цикле инерциалки ускорения по осям ax ay и dt а в цикле синхронизации жпс данных получать суммированием из ускорений и dt дельту скорости и позиции за заданное время задержки жпс данных (за время соотвествующее сумме dt), но на апм такое просадило бы проц и заняло бы существенный кусок оперативы - поэтому сохраняю готовые ускорения и перемещения с интервалом поступления ГНСС данных

  20. #6097

    Регистрация
    26.11.2012
    Адрес
    Tambov
    Возраст
    46
    Сообщений
    777
    Цитата Сообщение от alexeykozin Посмотреть сообщение
    дельта инс не имеет интегратора,
    Т.е. фактически у Вас не дельта скорости на выходе а дельта ускорения.. а прибавляете вы ее, если не ошибаюсь, к расстоянию по жпс..... как то не клеится (??)..
    Ну да ладно, а результат проверки этого алгоритма в полете можете прокоментировать ? лучше стало ? (насколько)..

  21. #6098

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    Цитата Сообщение от oleg70 Посмотреть сообщение
    Т.е. фактически у Вас не дельта скорости на выходе а дельта ускорения..
    в настоящее время беру и скорость и текущую позицию прямо из расчетов инерциалки
    сохряняю текущую позицию и скрость в буффер "снизу" это проталкивает данные наверх, сэмплов 5, шестой затирается.
    https://github.com/kozinalexey/alter...alNav.cpp#L255

    Код:
     
    _hp_x.push_back(_position.x); //save current inav position to buffer back point for calculate position change during gps delay
    _hp_y.push_back(_position.y);
    а тут я считаю разницу между тем что было по состоянию на момент актуальности текущих но запаздывающих жпс данных и тем что сейчас

    https://github.com/kozinalexey/alter...alNav.cpp#L268
    Код:
    _gps_position_lag_x = _position.x - _hp_x.peek(_gps_sample_number); //peek 0 400ms peek 1 300ms etc
    _gps_position_lag_y = _position.y - _hp_y.peek(_gps_sample_number);
    но на быстрых процах можно было бы из ускорений все считать
    Цитата Сообщение от oleg70 Посмотреть сообщение
    Ну да ладно, а результат проверки этого алгоритма в полете можете прокоментировать ? лучше стало ? (насколько)..
    визуально сильно снизилась тенденция к раскрутке по спирали при ошибке компаса, при анализе логов уменьшилось расхождение между координатами ГНСС и INAV. я вывел в лог в пакете INAV параметры LA LN с тем чтобы можно было сравнивать инерциальные координаты с данными навигационного приемника gps LAT LNG прямо в анализаторе логов мишен планера

  22. #6099

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от oleg70 Посмотреть сообщение
    Т.е. фактически у Вас не дельта скорости на выходе а дельта ускорения.. а прибавляете вы ее, если не ошибаюсь, к расстоянию по жпс..... как то не клеится (??)..
    да нет, всё клеится РастИНС1-РастИНС0 = дельтаРастИНС, это как раз и есть расчет ИНСки между отсчетами ГНСС, т.е. актуализация положения между отсчетами ГНСС. дельтаРастИНС/dt = СкоростьИНС (за период dt); где dt - период получения данных ГНСС. Алексей, коэффициент притяжки ИНС и период обновления ИНС какие?

  23. #6100

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    Цитата Сообщение от rual Посмотреть сообщение
    Алексей, коэффициент притяжки ИНС и период обновления ИНС какие?
    _gps_k_gps_spd =0.003 к 0.997 (подтяг скорости )
    _gps_k_gps_pos =0.008 к 0.992 (подтяг позиции )

    осуществляется с частотой около 100 гц
    Код:
    	
    _position.x = _position.x * _gps_k_inav_pos +  (_gps_position.x + _gps_position_lag_x) * _gps_k_gps_pos; //pool inav position to gps position
    _position.y = _position.y * _gps_k_inav_pos +  (_gps_position.y + _gps_position_lag_y) * _gps_k_gps_pos;
    
    
    _velocity.x = _velocity.x * _gps_k_inav_spd + (_gps_velocity_x + _gps_velocity_lag_x) * _gps_k_gps_spd ; //pool inav velocity to gps velocity
    _velocity.y = _velocity.y * _gps_k_inav_spd + (_gps_velocity_y + _gps_velocity_lag_y) * _gps_k_gps_spd ;
    но имеет смысл после внедрения компенсациипопробовать подтяг скорости усилить до 0.005 к 0.995
    параметры подтяга и компенсации жпс лага - ругулируемые из парметров, привел значения которые были использованы в полете на видео

  24. #6101

    Регистрация
    26.11.2012
    Адрес
    Tambov
    Возраст
    46
    Сообщений
    777
    Цитата Сообщение от alexeykozin Посмотреть сообщение
    к раскрутке по спирали при ошибке компаса,
    компас, я так понял, смешивается с гирой и акселем в общий алгоритм ориентации ИНС (типа алго магвика и махони)??
    Цитата Сообщение от alexeykozin Посмотреть сообщение
    визуально сильно снизилась тенденция к раскрутке
    тогда - это результат (!), поздравляю..

  25. #6102

    Регистрация
    26.11.2012
    Адрес
    Tambov
    Возраст
    46
    Сообщений
    777
    Я тут был воодушевлён применением RPi в качестве контроллера полета.. Так вот последние изыскания показали, что при работе с SPI многозадачный линукс становится фактически однозадачным... Иными словами - запустив в одном терминале процесс обмена данными по SPI и их выводом на экран, другие задачи запускаться не собираются до тех пор пока мы не убьём этот процесс...
    Вывод: нафига тогда всё это (?), и вопрос по передаче видео по WiFi и тому подобное отпадает сам собой.. Не понятно как люди, делающие платы типа "NAVIO" надеются на развитие этого направления (?).
    Там правда у них какойто "патч" линукса.. и похоже I2C... - но это, по моему, только усугубляет проблему..

  26. #6103

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от oleg70 Посмотреть сообщение
    Так вот последние изыскания показали, что при работе с SPI многозадачный линукс становится фактически однозадачным... Иными словами - запустив в одном терминале процесс обмена данными по SPI и их выводом на экран, другие задачи запускаться не собираются до тех пор пока мы не убьём этот процесс...
    Дык там похоже внутри процесса бесконечный опрос битов в регистре СПИ, надо код драйвера прошерстить, в момент ожидания должна вызываться спецфункция передачи управления ОС.

  27. #6104

    Регистрация
    26.11.2012
    Адрес
    Tambov
    Возраст
    46
    Сообщений
    777
    Цитата Сообщение от rual Посмотреть сообщение
    надо код драйвера прошерстить,
    Да, тут без "хирургического вмешательства" не обойтись, только для такого меня этот процесс может надолго затянуться.. проще наверно дождаться когда "гурманы-операционьщики" сделают нормальную РТОС для этих целей.

  28. #6105

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    имхо напрасное это дело возлагать на бортовой компьютер функции по стабилизации полета и управлении моторами.
    куда более было бы просто оставить стабилизацию на хорошем проверенном апм а функции навигации реализовать на бортовом компе
    втч визуальную ориентацию. требуемые для полета угловые поправки можно передавать по мавлинку через сериал

  29. #6106

    Регистрация
    26.11.2012
    Адрес
    Tambov
    Возраст
    46
    Сообщений
    777
    Цитата Сообщение от alexeykozin Посмотреть сообщение
    а функции навигации реализовать на бортовом компе
    втч визуальную ориентацию.
    Всю навигацию, любого уровня сложности, stm обсчитывает не напрягаясь... Я у себя ему и RTOS и OSD навесил + 500 гц реалтайм, а ему (stmF4) всё ни по чём, только ОЗУ уже впритык... А визуальная ориентация - больше мечты, чем реальность, пока даже прототипов не видно на просторах инета.
    От RPi хотелось только одного - чтоб весь софт был на языке высокого уровня, чтоб без всякой компиляции/перепрошивки и прочей колготы с программаторами и средами разработки, чуть ли не в поле, испытывать те или иные идеи алгоритмов управления, навешивать новые сенсоры, и т.д .. пока что глухо с этой затеей (на полку, до лучших времен)

  30. #6107

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    Цитата Сообщение от oleg70 Посмотреть сообщение
    Всю навигацию, любого уровня сложности, stm обсчитывает не напрягаясь...
    за исключением визуального ориентирования, именно его я и имел ввиду под внешней навигационной системой.

  31. #6108

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Привет Друзья и Коллеги по хобби! Пользуясь случаем поздравляю всех с наступающим Новым годом! Здоровья крепкого Вам и родным, удачи во всех начинаниях, мирного неба! Высоких и красивых полётов!

  32. #6109

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    С наступающим Новым Годом!

  33. #6110

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Всем здравия в новом году!
    Шавет нужен:
    Короче писал-писал драйвер да не выписал, точнее драйвер то есть, но случилось беда - при экспериментах с мелкой уложил 6000 и лапу проца - но не суть, куплю - приедут. Как говорится "плохой результат-тоже результат" )))
    Суть в другом, пока это всё ехать собирается, на мелкой накопилось куча косяков и вопросов которые надо решать:
    1) и самое основное - что делать? (риторический вопрос) т.к. я-мы убедились, что компас в 9250 - нехорошее железо и связываться с ним не хочется - есть 2 решения данной беды:
    а) вешать компас через 9250 или оставить через 6000 (это всё не проверено и недоказуемо)
    б) вешать компас на spi (HMC5983) забирать обе лапы с 6000 - тем самым забыть о 2- imu
    2) сделать плату чуть шире скажем не 28Х28 а 28Х32 - будет место поставить MINI-USB рядом с выходами на моторы, а не микро как сейчас...

  34. #6111

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    6000 есть с донора, если нужно для опытов не вопрос.
    компас надо либо вешать на спи либо понижать частоту айтуси
    второй иму будет интересен, надо поискать другие свободные лапы.
    юсб можно вообще убрать с мини платы. все вывести на мкроплощадки для распайки на тонкие проводки,
    на микроаппарататах каждый грамм на счету кому нужен юсб припаяются
    миниюсб вообще лошадь и не вариант

  35. #6112

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Цитата Сообщение от alexeykozin Посмотреть сообщение
    6000 есть с донора, если нужно для опытов не вопрос.
    "Золушка в 12 часов превратилась в тыкву, но принца уже было не остановить" ))) я уже с ф4бы выдрал - всё пучком...
    лап нет сапсем ))) - экспериментирую дальше )))

  36. #6113

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    Всем здравия в новом году!
    Привет! С Наступившим!
    Цитата Сообщение от SergDoc Посмотреть сообщение
    что компас в 9250 - нехорошее железо и связываться с ним не хочется
    Делись, чем оно плохо? А то у меня есть платка с ней, но пока не разбирался.

    Цитата Сообщение от SergDoc Посмотреть сообщение
    а) вешать компас через 9250 или оставить через 6000 (это всё не проверено и недоказуемо)
    б) вешать компас на spi (HMC5983) забирать обе лапы с 6000 - тем самым забыть о 2- imu
    Вешать лучше отдельно (не через МПУ), ибо ты сам написал почему. хотя... надо бы проверить сквозное чтение.
    2 датчика нужны? а зачем? если только в плане отказоустойчивости, но прецедентов в полёте ведь не было? если смысл удорожания и нагромождения?

    Цитата Сообщение от SergDoc Посмотреть сообщение
    2) сделать плату чуть шире скажем не 28Х28 а 28Х32 - будет место поставить MINI-USB рядом с выходами на моторы, а не микро как сейчас...
    вот мне понравилась компоновка 20х40 http://www.banggood.com/Mini-NAZE32-...p-1021113.html
    но правда это Ф3, там назе32шная дурь с внешним юсб-юсарт, можно поставить Ф4 и убрать мостик, и ещё флэху спиашную поставить

  37. #6114

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Цитата Сообщение от rual Посмотреть сообщение
    Делись, чем оно плохо? А то у меня есть платка с ней, но пока не разбирался.
    грубо: если данные с hmc мне надо делить на 1095, то АК надо умножать на 1.5 ( вчера всё-таки заставил 6000 -ю упираться в аукс и2ц - надо осцилл на работе забрать теперь...

    Цитата Сообщение от rual Посмотреть сообщение
    и ещё флэху спиашную поставить
    мы с Максом думали на воткнуть что-нибудь вместо sd, но чёт ничего серьёзного (в плане объёма) и дешевого нет - легче sd припаять))) в мелкой пока связка фрам+сд не мешают)))

  38. #6115

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    недостатки сд карт.
    -кардхолдер занимает много места
    -содержит намагничиваемый металл и если рядом с компасом то после касания его отверткой может отравлять компасу жизнь
    -содержит котнтакты нужно осторожничать с промывкой, может насосать в себя раствора промывочной жидкости и тогда привет
    в логике арду заложено что если карта не вставлена при запуске то только консоль - значит нужен будет джампер при отказе от сд

  39. #6116

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Цитата Сообщение от alexeykozin Посмотреть сообщение
    в логике арду заложено что если карта не вставлена при запуске то только консоль - значит нужен будет джампер при отказе от сд
    ну его - эту логику:
    1) если что-то не так вываливаться в консоль
    2) желательно при перепрошивке стирать фрам (задать буту такую возможность) что на мой взгляд лучше всего - никаких проблем с параметрами после прошивки...
    3) из станции стирать фрам с ребутом
    например в таулабсе в отличии от опенпилота можно флеш стирать из станции, а не заливать отдельную прошивку для этого...
    а так да особенно на мелкой - кусок железа на всё брюхо...

  40. #6117

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    И так, HMC5883 пишется и читается через MPU6000, одна проблема - потерял ось X компаса (
    Код:
    /**
     ******************************************************************************
     * @addtogroup PIOS PIOS Core hardware abstraction layer
     * @{
     * @addtogroup PIOS_MPU6000M MPU6000M Functions
     * @brief Deals with the hardware interface to the 3-axis gyro
     * @{
     *
     * @file       pios_mpu6000m.c
     * @author     Swift-Flyer, http://swift-flyer.com, Copyright (C) 2015
     * @brief      MPU6000M 9-axis gyro accel and mag chip
     * @see        The GNU Public License (GPL) Version 3
     *
     ******************************************************************************
     */
    /* 
     * This program is free software; you can redistribute it and/or modify 
     * it under the terms of the GNU General Public License as published by 
     * the Free Software Foundation; either version 3 of the License, or 
     * (at your option) any later version.
     * 
     * This program is distributed in the hope that it will be useful, but 
     * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
     * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
     * for more details.
     * 
     * You should have received a copy of the GNU General Public License along 
     * with this program; if not, write to the Free Software Foundation, Inc., 
     * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
     */
    
    /* Project Includes */
    #include "pios.h"
    
    #if defined(PIOS_INCLUDE_MPU6000M)
    
    #include "physical_constants.h"
    #include "pios_mpu6000m.h"
    #include "pios_semaphore.h"
    #include "pios_thread.h"
    #include "pios_queue.h"
    
    /* Private constants */
    #define MPU6000M_TASK_PRIORITY    PIOS_THREAD_PRIO_HIGHEST
    #define MPU6000M_TASK_STACK_BYTES 512
    #define PIOS_MPU6000M_MAX_DOWNSAMPLE 2
    
    #define MPU6000M_WHOAMI_ID       0x68
    
    #ifdef PIOS_MPU6000M_SPI_HIGH_SPEED
    #define MPU6000M_SPI_HIGH_SPEED              PIOS_MPU6000M_SPI_HIGH_SPEED
    #else
    #define MPU6000M_SPI_HIGH_SPEED              20000000    // should result in 10.5MHz clock on F4 targets like Sparky2
    #endif
    #define MPU6000M_SPI_LOW_SPEED               1000000
    
    
    
    /* HMC5883 Addresses */
    #define PIOS_MPU6000M_HMC5883_I2C_ADDR            0x1E
    #define PIOS_MPU6000M_HMC5883_I2C_READ_ADDR      0x3D
    #define PIOS_MPU6000M_HMC5883_I2C_WRITE_ADDR     0x3C
    #define PIOS_MPU6000M_HMC5883_CONFIG_REG_A        (uint8_t)0x00
    #define PIOS_MPU6000M_HMC5883_CONFIG_REG_B        (uint8_t)0x01
    #define PIOS_MPU6000M_HMC5883_MODE_REG            (uint8_t)0x02
    #define PIOS_MPU6000M_HMC5883_DATAOUT_XMSB_REG        0x03
    #define PIOS_MPU6000M_HMC5883_DATAOUT_XLSB_REG        0x04
    #define PIOS_MPU6000M_HMC5883_DATAOUT_ZMSB_REG        0x05
    #define PIOS_MPU6000M_HMC5883_DATAOUT_ZLSB_REG        0x06
    #define PIOS_MPU6000M_HMC5883_DATAOUT_YMSB_REG        0x07
    #define PIOS_MPU6000M_HMC5883_DATAOUT_YLSB_REG        0x08
    #define PIOS_MPU6000M_HMC5883_DATAOUT_STATUS_REG        0x09
    #define PIOS_MPU6000M_HMC5883_DATAOUT_IDA_REG        0x0A
    #define PIOS_MPU6000M_HMC5883_DATAOUT_IDB_REG        0x0B
    #define PIOS_MPU6000M_HMC5883_DATAOUT_IDC_REG        0x0C
    
    /* Output Data Rate */
    #define PIOS_MPU6000M_HMC5883_ODR_0_75        0x00
    #define PIOS_MPU6000M_HMC5883_ODR_1_5        0x04
    #define PIOS_MPU6000M_HMC5883_ODR_3            0x08
    #define PIOS_MPU6000M_HMC5883_ODR_7_5        0x0C
    #define PIOS_MPU6000M_HMC5883_ODR_15            0x10
    #define PIOS_MPU6000M_HMC5883_ODR_30            0x14
    #define PIOS_MPU6000M_HMC5883_ODR_75            0x18
    
    /* Measure configuration */
    #define PIOS_MPU6000M_HMC5883_MEASCONF_NORMAL        0x00
    #define PIOS_MPU6000M_HMC5883_MEASCONF_BIAS_POS        0x01
    #define PIOS_MPU6000M_HMC5883_MEASCONF_BIAS_NEG        0x02
    
    /* Gain settings */
    #define PIOS_MPU6000M_HMC5883_GAIN_0_88            0x00
    #define PIOS_MPU6000M_HMC5883_GAIN_1_3            0x20
    #define PIOS_MPU6000M_HMC5883_GAIN_1_9            0x40
    #define PIOS_MPU6000M_HMC5883_GAIN_2_5            0x60
    #define PIOS_MPU6000M_HMC5883_GAIN_4_0            0x80
    #define PIOS_MPU6000M_HMC5883_GAIN_4_7            0xA0
    #define PIOS_MPU6000M_HMC5883_GAIN_5_6            0xC0
    #define PIOS_MPU6000M_HMC5883_GAIN_8_1            0xE0
    
    /* Modes */
    #define PIOS_MPU6000M_HMC5883_MODE_CONTINUOUS    0x00
    #define PIOS_MPU6000M_HMC5883_MODE_SINGLE        0x01
    #define PIOS_MPU6000M_HMC5883_MODE_IDLE            0x02
    #define PIOS_MPU6000M_HMC5883_MODE_SLEEP            0x03
    /* Global Variables */
    
    enum pios_mpu6000m_dev_magic {
        PIOS_MPU6000M_DEV_MAGIC = 0x9da9b3ed,
    };
    
    struct mpu6000m_dev {
        uint32_t spi_id;
        uint32_t slave_num;
        enum pios_mpu60x0_accel_range accel_range;
        enum pios_mpu60x0_range gyro_range;
        struct pios_queue *gyro_queue;
        struct pios_queue *accel_queue;
        struct pios_queue *mag_queue;
        struct pios_thread *TaskHandle;
        struct pios_semaphore *data_ready_sema;
        const struct pios_mpu6000m_cfg *cfg;
        enum pios_mpu60x0_filter filter;
        enum pios_mpu6000m_dev_magic magic;
    };
    
    //! Global structure for this device device
    static struct mpu6000m_dev *dev;
    
    //! Private functions
    static struct mpu6000m_dev *PIOS_MPU6000M_alloc(const struct pios_mpu6000m_cfg *cfg);
    static int32_t PIOS_MPU6000M_Validate(struct mpu6000m_dev *dev);
    static void PIOS_MPU6000M_Task(void *parameters);
    static uint8_t PIOS_MPU6000M_ReadReg(uint8_t reg);
    static int32_t PIOS_MPU6000M_WriteReg(uint8_t reg, uint8_t data);
    static int32_t PIOS_MPU6000M_ClaimBus(bool lowspeed);
    static int32_t PIOS_MPU6000M_ReleaseBus(bool lowspeed);
    
    /**
     * @brief Allocate a new device
     */
    static struct mpu6000m_dev *PIOS_MPU6000M_alloc(const struct pios_mpu6000m_cfg *cfg)
    {
        struct mpu6000m_dev *mpu6000m_dev;
    
        mpu6000m_dev = (struct mpu6000m_dev *)PIOS_malloc(sizeof(*mpu6000m_dev));
        if (!mpu6000m_dev)
            return NULL;
    
        mpu6000m_dev->magic = PIOS_MPU6000M_DEV_MAGIC;
    
        mpu6000m_dev->accel_queue = PIOS_Queue_Create(PIOS_MPU6000M_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_accel_data));
        if (mpu6000m_dev->accel_queue == NULL) {
            PIOS_free(mpu6000m_dev);
            return NULL;
        }
    
        mpu6000m_dev->gyro_queue = PIOS_Queue_Create(PIOS_MPU6000M_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_gyro_data));
        if (mpu6000m_dev->gyro_queue == NULL) {
            PIOS_Queue_Delete(dev->accel_queue);
            PIOS_free(mpu6000m_dev);
            return NULL;
        }
    
        if (cfg->use_magnetometer) {
            mpu6000m_dev->mag_queue = PIOS_Queue_Create(PIOS_MPU6000M_MAX_DOWNSAMPLE, sizeof(struct pios_sensor_mag_data));
            if (mpu6000m_dev->mag_queue == NULL) {
                PIOS_Queue_Delete(dev->accel_queue);
                PIOS_Queue_Delete(dev->gyro_queue);
                PIOS_free(mpu6000m_dev);
                return NULL;
            }
        }
    
        mpu6000m_dev->data_ready_sema = PIOS_Semaphore_Create();
        if (mpu6000m_dev->data_ready_sema == NULL) {
            PIOS_Queue_Delete(dev->accel_queue);
            PIOS_Queue_Delete(dev->gyro_queue);
            if (cfg->use_magnetometer)
                PIOS_Queue_Delete(dev->mag_queue);
            PIOS_free(mpu6000m_dev);
            return NULL;
        }
    
        return mpu6000m_dev;
    }
    
    /**
     * @brief Validate the handle to the device
     * @returns 0 for valid device or -1 otherwise
     */
    static int32_t PIOS_MPU6000M_Validate(struct mpu6000m_dev *dev)
    {
        if (dev == NULL)
            return -1;
        if (dev->magic != PIOS_MPU6000M_DEV_MAGIC)
            return -2;
        if (dev->spi_id == 0)
            return -3;
        return 0;
    }
    
    /**
     * @brief Claim the SPI bus for the communications and select this chip
     * \param[in] flag controls if low speed access for control registers should be used
     * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
     */
    static int32_t PIOS_MPU6000M_ClaimBus(bool lowspeed)
    {
        if (PIOS_MPU6000M_Validate(dev) != 0)
            return -1;
    
        if (PIOS_SPI_ClaimBus(dev->spi_id) != 0)
            return -2;
    
        if (lowspeed)
            PIOS_SPI_SetClockSpeed(dev->spi_id, MPU6000M_SPI_LOW_SPEED);
    
        PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
    
        return 0;
    }
    
    /**
     * @brief Release the SPI bus for the communications and end the transaction
     * \param[in] must be true when bus was claimed in lowspeed mode
     * @return 0 if successful
     */
    static int32_t PIOS_MPU6000M_ReleaseBus(bool lowspeed)
    {
        if (PIOS_MPU6000M_Validate(dev) != 0)
            return -1;
    
        PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
    
        if (lowspeed)
            PIOS_SPI_SetClockSpeed(dev->spi_id, MPU6000M_SPI_HIGH_SPEED);
    
        PIOS_SPI_ReleaseBus(dev->spi_id);
    
        return 0;
    }
    
    /**
     * @brief Read a register from MPU6000M
     * @returns The register value
     * @param reg[in] Register address to be read
     */
    static uint8_t PIOS_MPU6000M_ReadReg(uint8_t reg)
    {
        uint8_t data;
    
        PIOS_MPU6000M_ClaimBus(true);
    
        PIOS_SPI_TransferByte(dev->spi_id, 0x80 | reg); // request byte
        data = PIOS_SPI_TransferByte(dev->spi_id, 0);   // receive response
    
        PIOS_MPU6000M_ReleaseBus(true);
    
        return data;
    }
    
    /**
     * @brief Writes one byte to the MPU6000M register
     * \param[in] reg Register address
     * \param[in] data Byte to write
     * @returns 0 when success
     */
    static int32_t PIOS_MPU6000M_WriteReg(uint8_t reg, uint8_t data)
    {
        if (PIOS_MPU6000M_ClaimBus(true) != 0)
            return -1;
    
        PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg);
        PIOS_SPI_TransferByte(dev->spi_id, data);
    
        PIOS_MPU6000M_ReleaseBus(true);
    
        return 0;
    }
    
    /**
     * @brief Writes one byte to the HMC5883 register using MPU6000M I2C master
     * \param[in] reg Register address
     * \param[in] data Byte to write
     * @returns 0 when success
     */
    static int32_t PIOS_MPU6000M_Mag_WriteReg(uint8_t reg, uint8_t data)
    {
        // we will use I2C SLV4 to manipulate with HMC5883 control registers
        if (PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_REG_REG, reg) != 0)
            return -1;
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_ADDR_REG, PIOS_MPU6000M_HMC5883_I2C_ADDR);
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_DO_REG, data);
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_CTRL_REG, PIOS_MPU60X0_I2CSLV_EN);
        uint32_t timeout = 0;
    
        // wait for I2C transaction done, use simple safety
        // escape counter to prevent endless loop in case
        // MPU6000M is broken
        uint8_t status = 0;
        do {
            if (timeout++ > 50)
                return -2;
    
            status = PIOS_MPU6000M_ReadReg(PIOS_MPU60X0_I2C_MST_STATUS_REG);
        } while ((status & PIOS_MPU60X0_I2C_MST_SLV4_DONE) == 0);
    
        if (status & PIOS_MPU60X0_I2C_MST_SLV4_NACK)
            return -3;
    
        return 0;
    }
    
    /**
     * @brief Reads one byte from the HMC5883 register using MPU6000M I2C master
     * \param[in] reg Register address
     * \param[in] data Byte to write
     **/
    static uint8_t PIOS_MPU6000M_Mag_ReadReg(uint8_t reg)
    {
        // we will use I2C SLV4 to manipulate with HMC5883 control registers
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_REG_REG, reg);
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_ADDR_REG, PIOS_MPU6000M_HMC5883_I2C_ADDR | 0x80);
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV4_CTRL_REG, PIOS_MPU60X0_I2CSLV_EN);
        uint32_t timeout = 0;
    
        // wait for I2C transaction done, use simple safety
        // escape counter to prevent endless loop in case
        // MPU6000M is broken
        uint8_t status = 0;
        do {
            if (timeout++ > 50)
                return 0;
    
            status = PIOS_MPU6000M_ReadReg(PIOS_MPU60X0_I2C_MST_STATUS_REG);
        } while ((status & PIOS_MPU60X0_I2C_MST_SLV4_DONE) == 0);
    
        return PIOS_MPU6000M_ReadReg(PIOS_MPU60X0_SLV4_DI_REG);
    }
    
    /**
     * @brief Initialize the HMC5883 magnetometer inside MPU6000M
     * \return 0 if success
     *
     */
    static int32_t PIOS_MPU6000M_Mag_Config(void)
    {
        char idm = PIOS_MPU6000M_Mag_ReadReg(PIOS_MPU6000M_HMC5883_DATAOUT_IDA_REG);
        
        if(idm != 'H') // Expect H
            
            return -2;
        // CRTL_REGA
        PIOS_MPU6000M_Mag_WriteReg(PIOS_MPU6000M_HMC5883_CONFIG_REG_A, 0x70);
        
        PIOS_DELAY_WaitmS(2);
        
        // CRTL_REGB
        PIOS_MPU6000M_Mag_WriteReg(PIOS_MPU6000M_HMC5883_CONFIG_REG_B, 0x20);
        
        PIOS_DELAY_WaitmS(2);
        
        // Mode register
        PIOS_MPU6000M_Mag_WriteReg(PIOS_MPU6000M_HMC5883_MODE_REG, 0x00);
        
        // give chip some time to initialize
        PIOS_DELAY_WaitmS(2);
    
        // configure mpu6000m to read hmc5x83 data range from register
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV0_REG_REG, PIOS_MPU6000M_HMC5883_DATAOUT_XMSB_REG);
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV0_ADDR_REG, PIOS_MPU6000M_HMC5883_I2C_ADDR | 0x80);
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SLV0_CTRL_REG, PIOS_MPU60X0_I2CSLV_EN | 6);
    
        return 0;
    }
    /**
     * @brief Read the identification bytes from the HMC5883 sensor
     * \param[out] uint8_t array of size 4 to store HMC5883 ID.
     * \return 0 if successful, -1 if not
     
    static uint8_t PIOS_MPU6000M_HMC5883_ReadID(uint8_t out)
    {
        uint8_t retval = PIOS_MPU6000M_Mag_ReadReg(PIOS_MPU6000M_HMC5883_DATAOUT_IDA_REG, 0);
        out = '\0';
        return retval;
    } */
    /**
     * @brief Initialize the MPU6000M gyro & accel registers
     * \return 0 if successful
     * \param[in] pios_mpu6000m_cfg struct to be used to configure sensor.
     *
     */
    static int32_t PIOS_MPU6000M_Config(struct pios_mpu6000m_cfg const *cfg)
    {
        // reset chip
        if (PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_PWR_MGMT_REG, PIOS_MPU60X0_PWRMGMT_IMU_RST) != 0)
            return -1;
    
        // give chip some time to initialize
        PIOS_DELAY_WaitmS(50);
    
        uint8_t id = PIOS_MPU6000M_ReadReg(PIOS_MPU60X0_WHOAMI);
        if (id != MPU6000M_WHOAMI_ID)
            return -2;
        
        
        // power management config
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_PWR_MGMT_REG, PIOS_MPU60X0_PWRMGMT_PLL_X_CLK);
    
        // user control
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_USER_CTRL_REG, PIOS_MPU60X0_USERCTL_DIS_I2C | PIOS_MPU60X0_USERCTL_I2C_MST_EN);
    
        if (dev->cfg->use_magnetometer)
            if (PIOS_MPU6000M_Mag_Config() != 0)
                return -3;
        
            
        
            
        // Digital low-pass filter and scale
        // set this before sample rate else sample rate calculation will fail
        
         PIOS_MPU6000M_SetLPF(cfg->default_filter);
    
        // Sample rate
        PIOS_MPU6000M_SetSampleRate(cfg->default_samplerate);
    
        // Set the gyro scale
        PIOS_MPU6000M_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG);
    
        // Set the accel scale
        PIOS_MPU6000M_SetAccelRange(PIOS_MPU60X0_ACCEL_8G);
    
        // Interrupt configuration
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_INT_CFG_REG, cfg->interrupt_cfg);
    
        // Interrupt enable
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_INT_EN_REG, PIOS_MPU60X0_INTEN_DATA_RDY);
    
        return 0;
    }
    
    /**
     * @brief Initialize the MPU6000M 9-axis sensor.
     * @return 0 for success, -1 for failure to allocate, -10 for failure to get irq
     */
    int32_t PIOS_MPU6000M_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000m_cfg *cfg)
    {
        dev = PIOS_MPU6000M_alloc(cfg);
        if (dev == NULL)
            return -1;
    
        dev->spi_id = spi_id;
        dev->slave_num = slave_num;
        dev->cfg = cfg;
    
        /* Configure the MPU6000M Sensor */
        if (PIOS_MPU6000M_Config(cfg) != 0)
            return -2;
    
        /* Set up EXTI line */
        PIOS_EXTI_Init(cfg->exti_cfg);
    
        // Wait 20 ms for data ready interrupt and make sure it happens
        // twice
        if ((PIOS_Semaphore_Take(dev->data_ready_sema, 20) != true) ||
            (PIOS_Semaphore_Take(dev->data_ready_sema, 20) != true)) {
            return -10;
        }
    
        dev->TaskHandle = PIOS_Thread_Create(
                PIOS_MPU6000M_Task, "pios_mpu6000m", MPU6000M_TASK_STACK_BYTES, NULL, MPU6000M_TASK_PRIORITY);
        PIOS_Assert(dev->TaskHandle != NULL);
    
        PIOS_SENSORS_Register(PIOS_SENSOR_ACCEL, dev->accel_queue);
        PIOS_SENSORS_Register(PIOS_SENSOR_GYRO, dev->gyro_queue);
    
        if (dev->cfg->use_magnetometer)
            PIOS_SENSORS_Register(PIOS_SENSOR_MAG, dev->mag_queue);
    
        return 0;
    }
    
    /**
     * @brief Test MPU6000M presence on the bus
     * @returns 0 if success
     */
    int32_t PIOS_MPU6000M_Test(void)
    {
        uint8_t id = PIOS_MPU6000M_ReadReg(PIOS_MPU60X0_WHOAMI);
        if (id != 0x68)
            return 1;
        char idm = PIOS_MPU6000M_Mag_ReadReg(PIOS_MPU6000M_HMC5883_DATAOUT_IDA_REG);
        
        if(idm != 'H') // Expect H
            
            return 1;
    
        return 0;
    }
    
    /**
     * @brief Set gyroscope range
     * @returns 0 if successful
     * @param range[in] gyroscope range
     */
    void PIOS_MPU6000M_SetGyroRange(enum pios_mpu60x0_range gyro_range)
    {
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_GYRO_CFG_REG, gyro_range);
    
        switch (gyro_range) {
        case PIOS_MPU60X0_SCALE_250_DEG:
            PIOS_SENSORS_SetMaxGyro(250);
            break;
        case PIOS_MPU60X0_SCALE_500_DEG:
            PIOS_SENSORS_SetMaxGyro(500);
            break;
        case PIOS_MPU60X0_SCALE_1000_DEG:
            PIOS_SENSORS_SetMaxGyro(1000);
            break;
        case PIOS_MPU60X0_SCALE_2000_DEG:
            PIOS_SENSORS_SetMaxGyro(2000);
            break;
        }
    
        dev->gyro_range = gyro_range;
        
    }
    
    /**
     * @brief Set accelerometer range
     * @returns 0 if success
     * @param range[in] accelerometer range
     */
    void PIOS_MPU6000M_SetAccelRange(enum pios_mpu60x0_accel_range accel_range)
    {
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_ACCEL_CFG_REG, accel_range);
            
        dev->accel_range = accel_range;
    
        
    }
    
    /**
     * @brief Set sampling frequency of accels and gyros axes
     * @returns 0 if successful
     * @param samplerate_hz[in] Sampling frequency in Hz
     */
    void PIOS_MPU6000M_SetSampleRate(uint16_t samplerate_hz)
    {
        // mpu6000m ODR divider is unable to run from 8kHz clock like mpu60x0 :(
        // check if someone want to use 250Hz DLPF and don't want 8kHz sampling
        // and politely refuse him
        uint16_t filter_frequency = 8000;
      
        if (dev->filter != PIOS_MPU60X0_LOWPASS_256_HZ)
            filter_frequency = 1000;
        
    
        
    
        // limit samplerate to filter frequency
        if (samplerate_hz > filter_frequency)
            samplerate_hz = filter_frequency;
    
        // calculate divisor, round to nearest integeter
        int32_t divisor = (int32_t)(((float)filter_frequency / samplerate_hz) + 0.5f) - 1;
    
        // limit resulting divisor to register value range
        if (divisor < 0)
            divisor = 0;
    
        if (divisor > 0xff)
            divisor = 0xff;
    
         PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_SMPLRT_DIV_REG, (uint8_t)divisor);
    }
    
    /**
     * Configure the digital low-pass filter
     */
    void PIOS_MPU6000M_SetLPF(enum pios_mpu60x0_filter filter)
    {
        PIOS_MPU6000M_WriteReg(PIOS_MPU60X0_DLPF_CFG_REG, filter);
    
        dev->filter = filter;
    }
    
    /**
     * @brief Get current gyro scale for deg/s
     * @returns scale
     */
    static float PIOS_MPU6000M_GetGyroScale(void)
    {
        switch (dev->gyro_range) {
        case PIOS_MPU60X0_SCALE_250_DEG:
            return 1.0f / 131.0f;
        case PIOS_MPU60X0_SCALE_500_DEG:
            return 1.0f / 65.5f;
        case PIOS_MPU60X0_SCALE_1000_DEG:
            return 1.0f / 32.8f;
        case PIOS_MPU60X0_SCALE_2000_DEG:
            return 1.0f / 16.4f;
        }
        return 0;
    }
    
    /**
     * @brief Get current gyro scale for ms^-2
     * @returns scale
     */
    static float PIOS_MPU6000M_GetAccelScale(void)
    {
        switch (dev->accel_range) {
        case PIOS_MPU60X0_ACCEL_2G:
            return GRAVITY / 16384.0f;
        case PIOS_MPU60X0_ACCEL_4G:
            return GRAVITY / 8192.0f;
        case PIOS_MPU60X0_ACCEL_8G:
            return GRAVITY / 4096.0f;
        case PIOS_MPU60X0_ACCEL_16G:
            return GRAVITY / 2048.0f;
        }
        return 0;
    }
    
    /**
    * @brief IRQ Handler.  Notice MPU6000M task to read all sensors data.
    */
    bool PIOS_MPU6000M_IRQHandler(void)
    {
        if (PIOS_MPU6000M_Validate(dev) != 0)
            return false;
    
        bool need_yield = false;
    
        PIOS_Semaphore_Give_FromISR(dev->data_ready_sema, &need_yield);
    
        return need_yield;
    }
    
    static void PIOS_MPU6000M_Task(void *parameters)
    {
        while (1) {
            //Wait for data ready interrupt
            if (PIOS_Semaphore_Take(dev->data_ready_sema, PIOS_SEMAPHORE_TIMEOUT_MAX) != true)
                continue;
    
            enum {
                IDX_REG = 0,
                IDX_ACCEL_XOUT_H,
                IDX_ACCEL_XOUT_L,
                IDX_ACCEL_YOUT_H,
                IDX_ACCEL_YOUT_L,
                IDX_ACCEL_ZOUT_H,
                IDX_ACCEL_ZOUT_L,
                IDX_TEMP_OUT_H,
                IDX_TEMP_OUT_L,
                IDX_GYRO_XOUT_H,
                IDX_GYRO_XOUT_L,
                IDX_GYRO_YOUT_H,
                IDX_GYRO_YOUT_L,
                IDX_GYRO_ZOUT_H,
                IDX_GYRO_ZOUT_L,
                IDX_MAG_XOUT_L,
                IDX_MAG_XOUT_H,
                IDX_MAG_YOUT_L,
                IDX_MAG_YOUT_H,
                IDX_MAG_ZOUT_L,
                IDX_MAG_ZOUT_H,
                BUFFER_SIZE,
            };
    
            uint8_t mpu6000m_rec_buf[BUFFER_SIZE];
            uint8_t mpu6000m_tx_buf[BUFFER_SIZE] = {PIOS_MPU60X0_ACCEL_X_OUT_MSB | 0x80, 0, 0, 0,
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    
            uint8_t transfer_size = (dev->cfg->use_magnetometer) ? BUFFER_SIZE : BUFFER_SIZE - 6;
            // claim bus in high speed mode
            if (PIOS_MPU6000M_ClaimBus(false) != 0)
                continue;
    
            if (PIOS_SPI_TransferBlock(dev->spi_id, mpu6000m_tx_buf, mpu6000m_rec_buf, transfer_size, 0) < 0) {
                PIOS_MPU6000M_ReleaseBus(false);
                continue;
            }
    
            PIOS_MPU6000M_ReleaseBus(false);
    
            struct pios_sensor_accel_data accel_data;
            struct pios_sensor_gyro_data gyro_data;
            struct pios_sensor_mag_data mag_data;
    
            float accel_x = (int16_t)(mpu6000m_rec_buf[IDX_ACCEL_XOUT_H] << 8 | mpu6000m_rec_buf[IDX_ACCEL_XOUT_L]);
            float accel_y = (int16_t)(mpu6000m_rec_buf[IDX_ACCEL_YOUT_H] << 8 | mpu6000m_rec_buf[IDX_ACCEL_YOUT_L]);
            float accel_z = (int16_t)(mpu6000m_rec_buf[IDX_ACCEL_ZOUT_H] << 8 | mpu6000m_rec_buf[IDX_ACCEL_ZOUT_L]);
            float gyro_x = (int16_t)(mpu6000m_rec_buf[IDX_GYRO_XOUT_H] << 8 | mpu6000m_rec_buf[IDX_GYRO_XOUT_L]);
            float gyro_y = (int16_t)(mpu6000m_rec_buf[IDX_GYRO_YOUT_H] << 8 | mpu6000m_rec_buf[IDX_GYRO_YOUT_L]);
            float gyro_z = (int16_t)(mpu6000m_rec_buf[IDX_GYRO_ZOUT_H] << 8 | mpu6000m_rec_buf[IDX_GYRO_ZOUT_L]);
            float mag_x = (int16_t)(mpu6000m_rec_buf[IDX_MAG_XOUT_H] << 8 | mpu6000m_rec_buf[IDX_MAG_XOUT_L]);
            float mag_y = (int16_t)(mpu6000m_rec_buf[IDX_MAG_YOUT_H] << 8 | mpu6000m_rec_buf[IDX_MAG_YOUT_L]);
            float mag_z = (int16_t)(mpu6000m_rec_buf[IDX_MAG_ZOUT_H] << 8 | mpu6000m_rec_buf[IDX_MAG_ZOUT_L]);
    
            // Rotate the sensor to TL convention.  The datasheet defines X as towards the right
            // and Y as forward. TL convention transposes this.  Also the Z is defined negatively
            // to our convention. This is true for accels and gyros. Magnetometer corresponds TL convention.
            switch (dev->cfg->orientation) {
            case PIOS_MPU6000M_TOP_0DEG:
                accel_data.y = accel_x;
                accel_data.x = accel_y;
                accel_data.z = -accel_z;
                gyro_data.y  = gyro_x;
                gyro_data.x  = gyro_y;
                gyro_data.z  = -gyro_z;
                mag_data.x   = mag_y;
                mag_data.y   = mag_x;
                mag_data.z   = -mag_z;
                break;
            case PIOS_MPU6000M_TOP_90DEG:
                accel_data.y = -accel_y;
                accel_data.x = accel_x;
                accel_data.z = -accel_z;
                gyro_data.y  = -gyro_y;
                gyro_data.x  = gyro_x;
                gyro_data.z  = -gyro_z;
                mag_data.x   = -mag_x;
                mag_data.y   = mag_y;
                mag_data.z   = -mag_z;
                break;
            case PIOS_MPU6000M_TOP_180DEG:
                accel_data.y = -accel_x;
                accel_data.x = -accel_y;
                accel_data.z = -accel_z;
                gyro_data.y  = -gyro_x;
                gyro_data.x  = -gyro_y;
                gyro_data.z  = -gyro_z;
                mag_data.x   = -mag_y;
                mag_data.y   = -mag_x;
                mag_data.z   = -mag_z;
    
                break;
            case PIOS_MPU6000M_TOP_270DEG:
                accel_data.y = accel_y;
                accel_data.x = -accel_x;
                accel_data.z = -accel_z;
                gyro_data.y  = gyro_y;
                gyro_data.x  = -gyro_x;
                gyro_data.z  = -gyro_z;
                mag_data.x   = mag_x;
                mag_data.y   = -mag_y;
                mag_data.z   = -mag_z;
                break;
            case PIOS_MPU6000M_BOTTOM_0DEG:
                accel_data.y = -accel_x;
                accel_data.x = accel_y;
                accel_data.z = accel_z;
                gyro_data.y  = -gyro_x;
                gyro_data.x  = gyro_y;
                gyro_data.z  = gyro_z;
                mag_data.x   = mag_y;
                mag_data.y   = -mag_x;
                mag_data.z   = mag_z;
                break;
    
            case PIOS_MPU6000M_BOTTOM_90DEG:
                accel_data.y = -accel_y;
                accel_data.x = -accel_x;
                accel_data.z = accel_z;
                gyro_data.y  = -gyro_y;
                gyro_data.x  = -gyro_x;
                gyro_data.z  = gyro_z;
                mag_data.x   = -mag_x;
                mag_data.y   = -mag_y;
                mag_data.z   = mag_z;
                break;
    
            case PIOS_MPU6000M_BOTTOM_180DEG:
                accel_data.y = accel_x;
                accel_data.x = -accel_y;
                accel_data.z = accel_z;
                gyro_data.y  = gyro_x;
                gyro_data.x  = -gyro_y;
                gyro_data.z  = gyro_z;
                mag_data.x   = -mag_y;
                mag_data.y   = mag_x;
                mag_data.z   = mag_z;
                break;
    
            case PIOS_MPU6000M_BOTTOM_270DEG:
                accel_data.y = accel_y;
                accel_data.x = accel_x;
                gyro_data.y  = gyro_y;
                gyro_data.x  = gyro_x;
                gyro_data.z  = gyro_z;
                accel_data.z = accel_z;
                mag_data.x   = mag_x;
                mag_data.y   = mag_y;
                mag_data.z   = mag_z;
                break;
    
            }
    
            int16_t raw_temp = (int16_t)(mpu6000m_rec_buf[IDX_TEMP_OUT_H] << 8 | mpu6000m_rec_buf[IDX_TEMP_OUT_L]);
            float temperature = 21.0f + ((float)raw_temp) / 333.87f;
    
            // Apply sensor scaling
            float accel_scale = PIOS_MPU6000M_GetAccelScale();
            accel_data.x *= accel_scale;
            accel_data.y *= accel_scale;
            accel_data.z *= accel_scale;
            accel_data.temperature = temperature;
    
            float gyro_scale = PIOS_MPU6000M_GetGyroScale();
            gyro_data.x *= gyro_scale;
            gyro_data.y *= gyro_scale;
            gyro_data.z *= gyro_scale;
            gyro_data.temperature = temperature;
    
            PIOS_Queue_Send(dev->accel_queue, &accel_data, 0);
            PIOS_Queue_Send(dev->gyro_queue, &gyro_data, 0);
    
            if (dev->cfg->use_magnetometer) {
                
                    mag_data.x /= 1090.0f;
                    mag_data.y /= 1090.0f;
                    mag_data.z /= 1090.0f;
                    PIOS_Queue_Send(dev->mag_queue, &mag_data, 0);
                
            }
        }
    }
    
    #endif
    
    /**
     * @}
     * @}
     */

  41. #6118

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    и все это включая утилизацию мпу ради одной ноги
    вместо того чтобы повесить 6000 и 5983 на одну spi с разными cs?

  42. #6119

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    ноги 2 - ещё прерывание и вообще сейчас висит 9250 и 6000+5883(2 набора, 2 cs, 2 прерывания) на проце задействованы все лапы - все, нету ни одной))))

    Цитата Сообщение от alexeykozin Посмотреть сообщение
    включая утилизацию мпу
    я чё первый раз датчики ломаю?)))
    ладно - надо понять где ось х делась....

  43. #6120

    Регистрация
    15.09.2011
    Адрес
    Москва
    Возраст
    45
    Сообщений
    5,916
    Записей в дневнике
    22
    ну так прерывания отродясь никто не использовал хотя все регулярно разводят. вот и две ноги. cs можно из 2 ног 4 девайса вытащить используя дешифратор но это лишняя микруха конечно
    ms5811 на той же spi?

+ Ответить в теме

Похожие темы

  1. Система стабилизации гиро+акселерометр
    от Фантомас в разделе Полеты по камере, телеметрия
    Ответов: 32
    Последнее сообщение: 25.01.2011, 14:47
  2. Продам Продам Клона Trex 450SEV2 + Аппаратура + Запчасти+ система стабилизации RTF
    от omegapraim в разделе Барахолка. Вертолеты
    Ответов: 1
    Последнее сообщение: 12.01.2011, 18:16
  3. Продам Трёхосевую систему стабилизации Turnigy V-Bar 600
    от avi@tor в разделе Барахолка. Аппаратура
    Ответов: 1
    Последнее сообщение: 08.11.2010, 13:02
  4. Продам Gaui система стабилизации GU365, дёшево.
    от avi@tor в разделе Барахолка. Вертолеты
    Ответов: 3
    Последнее сообщение: 03.08.2010, 11:13
  5. Системы стабилизации
    от max815 в разделе Фото и видеосъемка, системы стабилизации
    Ответов: 16
    Последнее сообщение: 11.03.2010, 03:14

Метки этой темы

Ваши права

  • Вы не можете создавать новые темы
  • Вы не можете отвечать в темах
  • Вы не можете прикреплять вложения
  • Вы не можете редактировать свои сообщения