10 Урок. Ориентация ОрбиКрафт 3D с помощью солнечных датчиков

tip

Прежде чем переходить к работе с солнечными датчиками советуем сначала изучить 08 Урок. Работа с ПИД-регулятором.

Солнечный датчик – это датчик, который позволяет определять расположение источника солнечного света. В модуле СД установлено четыре солнечных датчика, которые используются для реализации системы ориентации и стабилизации конструктора.

К каждому из датчиков можно обратиться по определенному адресу:

 * 1 датчик - 0x10 
 * 2 датчик - 0x11 
 * 3 датчик - 0x12 
 * 4 датчик - 0x13

Рисунок 1. Система кооринат солнечных датчиков конструктора

Как видно из чертежа, система координат конструктора и датчиков не совпадает, для того чтобы перевести координаты, полученные из датчиков в систему координат конструктора, используются матрицы поворота, для каждого датчика своя.

$$\mathrm{Для\; 1\; датчика}-\left[\begin{array}{ccc}0& 0& 1\\ 0& 1& 0\\ 1& 0& 0\end{array}\right]$$

$$\mathrm{Для\; 2\; датчика}-\left[\begin{array}{ccc}0& 0& 1\\ -1& 0& 0\\ 0& 1& 0\end{array}\right]$$

$$\mathrm{Для\; 3\; датчика}-\left[\begin{array}{ccc}0& 0& 1\\ 0& -1& 0\\ -1& 0& 0\end{array}\right]$$

$$\mathrm{Для\; 4\; датчика}-\left[\begin{array}{ccc}0& 0& 1\\ 1& 0& 0\\ 0& -1& 0\end{array}\right]$$

Программа ориентации по солнечным датчикам

Для использования показаний с солнечных датчиков в качестве входных значений для ПИД-регулятора надо, в первую очередь, научиться получать значения с этих датчиков. Для этого можно использовать функцию sunsensor_get_raw(i, vector)

где i – номер датчика от 0 до 3;

vector – указатель на массив int32_t, куда будут записаны показания по 3 осям.

Например:

int32_t v [3]; 
  sunsensor_get_raw(2, v); 

Теперь мы можем обратиться к показаниям третьего солнечного датчика (не забываем, нумерация с нуля, а значит номер 2 – это третий датчик):

v[0] – Показания по оси x;

v[1] – Показания по оси y;

v[2] – Показания по оси z.

После получения данных с датчика их надо перевести в систему координат конструктора. Для этого можно использовать несколько способов, например умножение координат на соответствующие коэффициенты в матрице поворота.

После этого значение надо преобразовать в угол к оси X. Этот угол уже можно использовать как входное значение для ПИД-регулятора.

Рассчитав управляющее значение, нам надо как-то передать его на УДМ – маховик – чтобы он изменил свою скорость вращения. Для удобного представления данных используется структура:

struct motor_torque 
{ 
    float torque;     uint16_t time; }__attribute__((packed)); 

В поле torque записывается управляющее значение ПИД-регулятора, а в поле time – значение задержки dt. Этот фрагмент кода позволяет заполнить структуру данными и послать сообщение на маховик. Для этого надо знать адрес маховика. В конструкторе маховики имеют адреса A, B и C в шестнадцатеричной системе счисления, что соответствует адресам 10, 11, 12 в десятеричной системе счисления.

tq.torque = PD_OUT; 
tq.time = (uint16_t)(dt * 1000.0); 
 
send_unican_message(11, 2576, (uint8_t*)&tq, sizeof(tq)); // 11 – адрес маховика В 

Кроме стабилизации, аппарат должен выполнять какую-то полезную задачу. Для выполнения кода полезной нагрузки параллельно с кодом стабилизации необходимо добавить в код дополнительный поток:

pthread_t thread; // Создаём поток 
pthread_create(&thread, NULL, payloadFunction, NULL); // Привязываем поток к функции  
void *payloadFunction(void *vargp)  // Функция, которая будет вызываться 
    {    
     
} 

Задание: создайте программу для ориентации ОрбиКрафт 3D по солнечным датчикам.