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

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

Создайте программу для ориентации ОрбиКрафт 3D.

Mag_orient.c

#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include "libschsat.h"
#include <pthread.h>
 
 
struct motor_torque
{
    float torque;
    uint16_t time;
}__attribute__((packed));
 
 
void *myThreadFun(void *vargp);
float alphaBeta(float omega, float dt, float phi);
float NormAngleDifference(float angleDifference);
 
 
float filtrt;
 
pthread_t thread;
 
int completePhoto = 0;
 
float phi_0 = 100;
 
 
 
void control(void){ 
 
    float dt = 0.1;
    float AVS[3];
    float mag_x = 0, mag_y = 0, mag_z = 0;
    uint16_t mag_num = 0;
    float motorSpeed = 0;
    struct motor_torque tq;
 
    float P = 3e-3;
    float D = 2e-2;
    float I = 0.0005;
    float I_sum = 0.004;
    float pid_border = 0.1;
    float PD_OUT = 0;
 
    float error = 0;
	float p_error = 0;
	float previous_error = 0;
	float intgral_error = 0;
	float DerrorDt = 0;
 
	float phi_current;
    float average;
    float absolute_angle;
    float privious_angle;
 
    int count = 0;
    int fixation = 0;
 
 
	magnetometer_gyro_set_telemetry_period(0, (uint16_t)(dt * 1000)); //Установить период телеметрии магнитометра и датчика угловых скоростей.
 
	motor_set_telemetry_period(0, (uint16_t)(dt * 1000)); //Установить период телеметрии маховика.
 
	motor_set_speed(0, 0); //Задать скорость вращения маховика.
 
    magnetometer_request_raw(mag_num, &mag_x, &mag_y, &mag_z); //Получить показания магнитометра.
 
 
    phi_current = atan2(mag_x, mag_y) * 180 / 3.14;
    privious_angle = phi_current;
    Sleep(dt);
 
	for (int i = 0; i < 20000; i++){
 
        gyro_request_raw(0, AVS, AVS + 1, AVS + 2); //Получить показания ДУС.
        magnetometer_request_raw(mag_num, &mag_x, &mag_y, &mag_z); //Получить показания магнитометра.
 
 
        phi_current = atan2(mag_x, mag_y) * 180 / 3.14;
        float angle_delta  = NormAngleDifference(phi_current - privious_angle);
 
 
        privious_angle = phi_current;
        absolute_angle += angle_delta;
        average = alphaBeta(AVS[2], dt, absolute_angle);
        phi_current = atan2(sin(average * 3.1415 / 180), cos(average * 3.1415 / 180)) * 180 / 3.1415;
 
 
        error = NormAngleDifference(phi_0 - phi_current);
 
        DerrorDt = (error - previous_error) / dt;
 
        intgral_error += error / dt;
 
        if(intgral_error * I > I_sum){
            intgral_error = I_sum / I;
        }
 
        if(intgral_error * I < -I_sum){
            intgral_error = -I_sum / I ;    
        }
 
 
        PD_OUT = error * P + DerrorDt * D + intgral_error * I;
 
        if(PD_OUT > pid_border){
            PD_OUT = pid_border;
        }
 
        if(PD_OUT < -pid_border){        
            PD_OUT = -pid_border;    
        }
 
 
 
        tq.torque = -PD_OUT;
        tq.time = (uint16_t)(dt * 1000.0);
 
        send_unican_message(10, 2576, (uint8_t*)&tq, sizeof(tq));
 
        // Debug commands for plots visualization
        // struct motor_torque tq_1;
        // struct motor_torque tq_2;
        // struct motor_torque tq_3;
 
        // tq_1.torque = phi_current;
        // tq_2.torque = average;
        // tq_3.torque = PD_OUT;
 
        // send_unican_message(11, 2576, &tq_1, sizeof(tq_1));
        // send_unican_message(12, 2576, &tq_2, sizeof(tq_2));
        // send_unican_message(13, 2576, &tq_3, sizeof(tq_3));
 
 
        motor_request_speed(0, &motorSpeed); //Получить скорость вращения маховика.
        previous_error = error;
        // printf("%f %f %f %f", phi_current, error, motorSpeed, PD_OUT);
        Sleep(dt);
 
        if (!fixation && (3 > fabs(error)) && (AVS[2] < 2))
        {
            if(count > 100){
                pthread_create(&thread, NULL, myThreadFun, NULL);
                count = 0;
                fixation = 1;
            }else{
                count++;
            }
        }
        if (completePhoto && (3 > fabs(error)) && (AVS[2] < 2))
        {
            if (count > 300){
                motor_set_speed(0, 0); //Задать скорость вращения маховика.
                return;
            }
            else{
                count++;
            }
        }
 
    }
    pthread_join(thread, NULL);
}
 
 
void *myThreadFun(void *vargp)
{
    camera_take_photo(1, 180);
    printf("Photo is taken");
    completePhoto = 1;
    phi_0 = -50;
}
 
 
float alphaBeta(float omega, float dt, float phi){
 
    static uint8_t first = 1;
    const float alpha = 0.99;
    float beta = 1 - alpha;
    if(first){
        filtrt = phi;
        first = 0;
        return phi;
    }
    filtrt = (filtrt + omega * dt) * alpha + phi * beta;
    return filtrt;
}
 
 
float NormAngleDifference(float angleDifference)
{
    if(angleDifference > 180)
        return angleDifference - 2 * 180;
    if(angleDifference < -180)
        return angleDifference + 2 * 180;
    return angleDifference;
}