10 Lesson. Orbicraft orientation by solar sensors

Download the program

Open the SUN file.py and enter into the data array data_sun = [] all data from the text data file; remove spaces and commas at the end of the array.

Python program code

import time
import math
 
kd = 200            # Differential feedback coefficient. If the satellite’s angular velocity is positive, than satellite must be spun clock-wise (i.e. the flywheel must rotated clock-wise).
kp = -100           # Proportional feedback coefficient. If the current angle is more than target angle, than satellite must be spun counter-clock-wise (i.e. the flywheel must rotated counter-clock-wise).
time_step = 0.05        # working time-step for the positioning and stabilization system
 
mtr_num = 1         # flywheel number
hyr_num = 1         # angular velocity sensor number
mag_num = 1         # magnetometer number
# Maximum allowed flywheel speed, rev/min
mtr_max_speed = 3000
sun_max = 25000     # Maximum value of the solar sensor (higher values are deemed as errors)
sun_min = 50        # Minimum value of the solar sensor (lower values are deemed as errors)
 
# To define the steering command for the flywheel it is critical to know not the angular orientation of the satellite as related to magnetic field but the angular error – the difference between the current angle and target angle alpha_goal
# By default the angle will be defined in range from -180 to 180 degree 
# angle_transformation function will change the angle measurement range. For the task of positioning the angle must be measured in the range from (alpha_goal-180) to (alpha_goal+180), where alpha_goal is the target angle between magnetometer’s x-axis and projection of magnetic field vector to horizontal plane.
 
def angle_transformation(alpha, alpha_goal):
    if alpha<=(alpha_goal - 180):
        alpha = alpha + 360
    elif alpha>(alpha_goal +180):
        alpha = alpha - 360
    return alpha
 
# Specifying the flywheel new speed 
# flywheel’s new speed equals to flywheel’s current speed + speed increment
# Speed increment is proportioned by angle error and angular velocity error
def motor_new_speed_PD(mtr_speed, alpha, alpha_goal, omega, omega_goal):
    mtr_new_speed = int(mtr_speed + kp*(alpha-alpha_goal) + kd*(omega-omega_goal))
    if mtr_new_speed > mtr_max_speed:
        mtr_new_speed = mtr_max_speed
    elif mtr_new_speed < -mtr_max_speed:
        mtr_new_speed = -mtr_max_speed
    return mtr_new_speed
 
def initialize_all():
    print "Enable motor №", mtr_num 
    motor_turn_on(mtr_num)
    sleep(1)
    print "Enable angular velocity sensor №", hyr_num 
    hyro_turn_on(hyr_num) # Switch on the angular velocity sensor
    sleep(1)
    print "Enable Sun sensors 1-4"
    sun_sensor_turn_on(1)
    sun_sensor_turn_on(2)
    sun_sensor_turn_on(3)
    sun_sensor_turn_on(4)
    sleep(1)
 
def switch_off_all():
    print "Finishing..."
    hyro_turn_off(hyr_num)   # Switch off the angular velocity sensor
    sun_sensor_turn_off(1)
    sun_sensor_turn_off(2)
    sun_sensor_turn_off(3)
    sun_sensor_turn_off(4)
    motor_set_speed(mtr_num, 0)
    sleep (1)
    motor_turn_off(mtr_num)
    print "Finish program"
 
def check_sun(sun11, sun12, sun21, sun22, sun31, sun32, sun41, sun42):
    delta1 = 0
    delta2 = 0 
    delta3 = 0 
    delta4 = 0 
    delta5 = 0
    delta6 = 0 
    delta7 = 0 
    delta8 = 0  
    delta_sum = 36000
    delta_min = 35000   
    delta_i = 0 
    data_sun = []
    for i in range(0, len(data_sun), 9):  # len(data_sun) - array length
        delta1 = abs(sun11 - data_sun[i+0])
        delta2 = abs(sun12 - data_sun[i+1])
        delta3 = abs(sun21 - data_sun[i+2])
        delta4 = abs(sun22 - data_sun[i+3])
        delta5 = abs(sun31 - data_sun[i+4])
        delta6 = abs(sun32 - data_sun[i+5])
        delta7 = abs(sun41 - data_sun[i+6])
        delta8 = abs(sun42 - data_sun[i+7])
        delta_sum = delta1 + delta2 + delta3 + delta4 + delta5 + delta6 + delta7 + delta8
        #print i, data_sun[i+7]
        #print "Sun_sens", sun11, sun12, sun21, sun22, sun31, sun32, sun41, sun42
        #print "deltas= ", delta1, delta2, delta3, delta4, delta5, delta6, delta7, delta8, delta_sum        
        if delta_min > delta_sum :
            delta_min = delta_sum
            delta_i = data_sun[i+8]
            #print "delta_min= ", delta_min, "delta_i= ", delta_i
    return delta_i
 
def sun_range(sun, sun_old):    
    if sun[1] > sun_max:
        sun[1] = sun_old[1]
    if sun[1] < sun_min:
        sun[1] = sun_old[1]
    if sun[2] > sun_max:
        sun[2] = sun_old[2]
    if sun[2] < sun_min:
        sun[2] = sun_old[2]     
    return sun
 
def control(): # Program’s main function
    initialize_all()
    # Initialize flywheel status
    mtr_state = 0
    # Initialize angular velocity sensor status     
    hyro_state = 0
    sun_sensor_num = 0     # Initialize variable for solar sensor number 
    sun_result_1 = [0,0,0] # Initialize sun_result_1
    sun_result_2 = [0,0,0] # Initialize sun_result_2
    sun_result_3 = [0,0,0] # Initialize sun_result_3
    sun_result_4 = [0,0,0] # Initialize sun_result_4
    sun_result_1_Old = [0,0,0] # Initialize sun_result_1_Old
    sun_result_2_Old = [0,0,0] # Initialize sun_result_2_Old
    sun_result_3_Old = [0,0,0] # Initialize sun_result_3_Old
    sun_result_4_Old = [0,0,0] # Initialize sun_result_4_Old    
 
    omega_goal = 0      # Target angular velocity
    alpha_goal = 30     # Target turning angle
 
    for i in range(300):
        # Put into memory the old values
        sun_result_1_Old =  sun_result_1    
        sun_result_2_Old =  sun_result_2        
        sun_result_3_Old =  sun_result_3        
        sun_result_4_Old =  sun_result_4        
        # sensors and flywheel data request
        sun_result_1 = sun_sensor_request_raw(1)
        sun_result_2 = sun_sensor_request_raw(2)            
        sun_result_3 = sun_sensor_request_raw(3)
        sun_result_4 = sun_sensor_request_raw(4)
        # print sun_result_1, sun_result_2, sun_result_3, sun_result_4
        # Checking the new measured value for going out of range 
        sun_result_1 = sun_range(sun_result_1, sun_result_1_Old)
        sun_result_2 = sun_range(sun_result_2, sun_result_2_Old)    
        sun_result_3 = sun_range(sun_result_3, sun_result_3_Old)    
        sun_result_4 = sun_range(sun_result_4, sun_result_4_Old)
        alpha_sun = check_sun(sun_result_1[1], sun_result_1[2], sun_result_2[1], sun_result_2[2], sun_result_3[1], sun_result_3[2], sun_result_4[1], sun_result_4[2])
        alpha_sun = angle_transformation(alpha_sun, alpha_goal)
        print alpha_sun
        hyro_state, gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) 
        mtr_state, mtr_speed = motor_request_speed(mtr_num)
        if not hyro_state: # if angular velocity sensor returned error code 0 (no error)
            gx_degs = gx_raw * 0.00875
            gy_degs = gy_raw * 0.00875
            gz_degs = gz_raw * 0.00875
            omega = gz_degs # if the angular velocity sensor sensor arranged via z-axis up, then satellite’s angular velocity matches to sensors’ readings of z-axis
            print "gx_degs =", gx_degs, "gy_degs =", gy_degs, "gz_degs =", gz_degs # Output data
elif hyro_state == 1: # if the sensor returned error code 1
            print "Fail because of access error, check the connection"
        elif hyro_state == 2: # if the sensor returned error code 2
            print "Fail because of interface error, check your code"
 
        if not mtr_state:   # if the flywheel returned error code 0 (no error)
            print "Motor_speed: ", mtr_speed
            mtr_new_speed = motor_new_speed_PD(mtr_speed,alpha_sun,alpha_goal,gz_degs,omega_goal)   # set the flywheel’s new speed 
            motor_set_speed(mtr_num, mtr_new_speed)
 
        sleep(time_step)
    switch_off_all()

Program code in C

#include "libschsat.h"
#include <stdio.h>
#include <stdint.h>
#include "libschsat.h"
#define LSS_OK 0 
#define LSS_ERROR 1 
#define LSS_BREAK 2
#include <math.h>
 
const int kd = 200; /*Incremental feedback coefficient. If the satellite’s angular velocity is positive, than satellite must be spun clock-wise (i.e. the flywheel must rotated clock-wise.*/
const int kp = -100; /*Proportional feedback coefficient. If the current angle is more than target angle, than satellite must be spun counter-clock-wise (i.e. the flywheel must rotated counter-clock-wise).*/
const float time_step = 0.05; //working time-step for the positioning and stabilization system 
const int mtr_max_speed = 3000;         // Maximum allowed flywheel speed, rev/min
const uint16_t mtr_num = 1;         // flywheel number
const uint16_t hyr_num = 1;                 // angular velocity sensor number
const uint16_t mag_num = 1;         // magnetometer number
 
const int sun_max = 25000;//Maximum value of the solar sensor (higher values are deemed as errors)
const int sun_min = 50;//Minimum value of the solar sensor (lower values are deemed as errors)
int16_t mtr_new_speed;
 
int angle_transformation(int alpha, int alpha_goal){
/* To define the steering command for the flywheel it is critical to know not the angular orientation of the satellite as related to magnetic field but the angular error – the difference between the current angle and target angle alpha_goal
By default the angle will be defined in range from -180 to 180 degree angle_transformation function will change the angle measurement range. For the task of positioning the angle must be measured in the range from (alpha_goal-180) to (alpha_goal+180), where alpha_goal is the target angle between magnetometer’s x-axis and projection of magnetic field vector to horizontal plane.
*/
    if (alpha <= (alpha_goal - 180)){
        alpha = alpha + 360;
    }
    else if (alpha>(alpha_goal +180)){
        alpha = alpha - 360;
    }
    return alpha;
}
 
int motor_new_speed_PD(int mtr_speed,int alpha,int alpha_goal, float omega, int omega_goal){
/* Specifying the flywheel new speed 
flywheel’s new speed equals to flywheel’s current speed + speed increment
Speed increment is proportioned by angle error and angular velocity error
*/
    int mtr_new_speed = mtr_speed + kp*(alpha-alpha_goal) + kd*(omega-omega_goal);
    if (mtr_new_speed > mtr_max_speed){
        mtr_new_speed = mtr_max_speed;
    }
    else if (mtr_new_speed < -mtr_max_speed){
        mtr_new_speed = -mtr_max_speed;
    }
    return mtr_new_speed;
}
 
void initialize_all(void){/* All systems’ initialization function*/
    printf("Enable motor №%d\n", mtr_num); 
    motor_turn_on(mtr_num);
    Sleep(1);
    printf("Enable angular velocity sensor №%d\n", hyr_num); 
    hyro_turn_on(hyr_num); // Enabling AVS
    Sleep(1);
    printf("Enable Sun sensors 1-4\n");
    sun_sensor_turn_on(1);
    sun_sensor_turn_on(2);
    sun_sensor_turn_on(3);
    sun_sensor_turn_on(4);
    Sleep(1);
}
 
void switch_off_all(void){/* All systems’ switching off function*/
    printf("Finishing...\n");
    int16_t new_speed = mtr_new_speed;
    hyro_turn_off(hyr_num); // Turning off AVS
    magnetometer_turn_off(mag_num);
    sun_sensor_turn_off(1);
    sun_sensor_turn_off(2);
    sun_sensor_turn_off(3);
    sun_sensor_turn_off(4);
    motor_set_speed(mtr_num, 0, &new_speed);
    Sleep(1);
    motor_turn_off(mtr_num);
    printf("Finish program\n");
}
 
int check_sun(int sun11, int sun12, int sun21, int sun22, int sun31, int sun32, int sun41, int sun42){ 
    int delta1 = 0;
    int delta2 = 0; 
    int delta3 = 0; 
    int delta4 = 0; 
    int delta5 = 0;
    int delta6 = 0; 
    int delta7 = 0; 
    int delta8 = 0; 
    int delta_sum = 36000;
    int delta_min = 35000;  
    int delta_i = 0; 
    float data_sun[] = {-47.329501295977, 239, 102, -48.1458278265892}; //insert data from text data file
    int i;
    for (i = 0; i < 4; i++){                 
        delta1 = abs(sun11 - data_sun[i]);
        delta2 = abs(sun12 - data_sun[i]);
        delta3 = abs(sun21 - data_sun[i]);
        delta4 = abs(sun22 - data_sun[i]);
        delta5 = abs(sun31 - data_sun[i]);
        delta6 = abs(sun32 - data_sun[i]);
        delta7 = abs(sun41 - data_sun[i]);
        delta8 = abs(sun42 - data_sun[i]);
        delta_sum = delta1 + delta2 + delta3 + delta4 + delta5 + delta6 + delta7 + delta8;
        printf("%d, %f\n", i, data_sun[i]);
        printf("Sun_sens: %d, %d, %d, %d, %d, %d, %d, %d\n", sun11, sun12, sun21, sun22, sun31, sun32, sun41, sun42);
        printf("deltas= %d, %d, %d, %d, %d, %d, %d, %d, %d\n", delta1, delta2, delta3, delta4, delta5, delta6, delta7, delta8, delta_sum);      
        if (delta_min > delta_sum) {
            delta_min = delta_sum;
            delta_i = data_sun[i];
            printf("delta_min= %d\n", delta_min);
            printf("delta_i= %d\n", delta_i);
        }
    }
    return delta_i;
}
 
int sun_range(uint16_t *sun_1, uint16_t *sun_2, uint16_t sun_old_1, uint16_t sun_old_2){    
 
    if (*sun_1 > sun_max){
        *sun_1 = sun_old_1;
    }
 
    if (*sun_1 < sun_min){
        *sun_1 = sun_old_1;
    }
    if (*sun_2> sun_max){
        *sun_2 = sun_old_2;
    }
    if (*sun_2 < sun_min){
        *sun_2 = sun_old_2;
    }       
    return 0;
}
 
int control(){ // Program’s main function
    initialize_all(); // Initialize flywheel status
    int mtr_state = 0;  // Initialize angular velocity sensor status
    int hyro_state = 0;
    //int mag_state = 0; // Initialize magnetometer status
    uint16_t sun_result_1[] = {0,0,0}; // Initialize sun_result_1
    uint16_t sun_result_2[] = {0,0,0}; // Initialize sun_result_2
    uint16_t sun_result_3[] = {0,0,0}; // Initialize sun_result_3
    uint16_t sun_result_4[] = {0,0,0}; // Initialize sun_result_4
    uint16_t sun_result_1_Old[] = {0,0,0}; // Initialize sun_result_1_Old
    uint16_t sun_result_2_Old[] = {0,0,0}; // Initialize sun_result_2_Old
    uint16_t sun_result_3_Old[] = {0,0,0}; // Initialize sun_result_3_Old
    uint16_t sun_result_4_Old[] = {0,0,0}; // Initialize sun_result_4_Old
    int16_t mtr_speed;
    //angular velocity sensor data
    int16_t gx_raw;
    int16_t gy_raw;
    int16_t gz_raw;
    int16_t *hyrox_raw = &gx_raw;
    int16_t *hyroy_raw = &gy_raw;
    int16_t *hyroz_raw = &gz_raw;
    //magnetometer data
    /*int16_t mgx;
    int16_t mgy;
    int16_t mgz;
    int16_t *magx_raw = &mgx;
    int16_t *magy_raw = &mgy; 
    int16_t *magz_raw = &mgz;*/
 
    int16_t omega;
    int omega_goal = 0;     // omega_goal - Target angular velocity
    int alpha_goal = 30;        // Target turning angle
    int i;
    int j;
    for (i = 0; i < 300; i++){
        // Put into memory the old values
        for(j = 0; j < 3; j++){
        sun_result_1_Old[j] = sun_result_1[j];  
        sun_result_2_Old[j] = sun_result_2[j];      
        sun_result_3_Old[j] = sun_result_3[j];      
        sun_result_4_Old[j] = sun_result_4[j];
        }       
        // sensors and flywheel data request
        sun_result_1[0] = sun_sensor_request_raw(1,&sun_result_1[1],&sun_result_1[2]);
        sun_result_2[0] = sun_sensor_request_raw(2,&sun_result_2[1],&sun_result_2[2]);          
        sun_result_3[0] = sun_sensor_request_raw(3,&sun_result_3[1],&sun_result_3[2]);
        sun_result_4[0] = sun_sensor_request_raw(4,&sun_result_4[1],&sun_result_4[2]);
        //print sun_result_1, sun_result_2, sun_result_3, sun_result_4
        // Checking the new measured value for going out of range 
        sun_result_1[0] = sun_range( & sun_result_1[1], & sun_result_1[2], sun_result_1_Old[1], sun_result_1_Old[2]);
        sun_result_2[0] = sun_range( & sun_result_2[1], & sun_result_2[2], sun_result_2_Old[1], sun_result_2_Old[2]);
        sun_result_3[0] = sun_range( & sun_result_3[1], & sun_result_3[2], sun_result_3_Old[1], sun_result_3_Old[2]);
        sun_result_4[0] = sun_range( & sun_result_4[1], & sun_result_4[2], sun_result_4_Old[1], sun_result_4_Old[2]);
        int alpha_sun = check_sun(sun_result_1[1], sun_result_1[2], sun_result_2[1], sun_result_2[2], sun_result_3[1], sun_result_3[2], sun_result_4[1], sun_result_4[2]);
        alpha_sun = angle_transformation(alpha_sun, alpha_goal);
        printf("alpha_sun = %d\n",alpha_sun);
 
        hyro_state = hyro_request_raw(hyr_num,hyrox_raw,hyroy_raw,hyroz_raw); 
        gx_raw = *hyrox_raw; 
        gy_raw = *hyroy_raw; 
        gz_raw = *hyroz_raw; 
        mtr_state = motor_request_speed(mtr_num, &mtr_speed);
        //mag_state = magnetometer_request_raw(mag_num, magx_raw, magy_raw, magz_raw);
        mtr_speed = motor_request_speed(mtr_num, &mtr_speed);
 
 
        if (!hyro_state){ // if angular velocity sensor returned error code 0 (no error)
            float gx_degs = gx_raw * 0.00875;
            float gy_degs = gy_raw * 0.00875;
            float gz_degs = gz_raw * 0.00875;
            omega = (int16_t)gz_degs;   // if the angular velocity sensor sensor arranged via z-axis up, then satellite’s angular velocity matches to sensors’ readings of z-axis
            printf("gx_degs = %f, gy_degs = %f, gz_degs = %f\n", gx_degs, gy_degs, gz_degs); // Data output
        }
        else if (hyro_state == 1){ // if the sensor returned error code 1
            printf("Fail because of access error, check the connection");
        }
        else if (hyro_state == 2){ // if the sensor returned error code 2
            printf("Fail because of interface error, check your code");
        }
 
        if (!mtr_state) { // if the flywheel returned error code 0 (no error)
            int16_t mtr_speed = 0;
            motor_request_speed(mtr_num, &mtr_speed);
            printf("Motor_speed: %d\n", mtr_speed); 
            // set the flywheel’s new speed 
            mtr_new_speed = motor_new_speed_PD(mtr_speed,alpha_sun, alpha_goal,omega,omega_goal );
            motor_set_speed(mtr_num, mtr_new_speed, &omega);
        }   
 
        Sleep(time_step);
    }
    switch_off_all();
    return 0;
}

The program procedure is as follows:

The values measured during the calibration of the solar data and the angular values are stored in a large data array. The check_sun function receives 8 values read from solar sensors (two from each sensor), calculates 8 deviation values delta1...delta8 as the absolute value of the remainder between the measured values and the values from the table. Then the total deviation of delta_sum is calculated. The program processes all the values from the array, looking for the minimum deviation of delta_sum with the corresponding angular value. As a result, the check_sun function returns the angular value at which the OrbiCraft turned. The sun_range function rejects erroneous values received from the solar sensor; if the measured value is above 25000 or below 50 (above sun_max or below sun_min), the program returns the old value from the acceptable range. Using this function allows you to correct measurement errors that sometimes occur. The main function of the control program reads the readings of the solar sensors 300 times per cycle, checking that the values do not exceed the range using the sun_range function. The correct values are passed to the check_sun function, which returns the angular value at which the OrbiCraft turned. Then, using the angle_transformation function, the angular value is converted to the range -180...180, and using the motor_new_speed_PD function, the new speed required for the rotation of the flywheels is calculated. Then the program is suspended for the duration of time_step.