User Tools

Site Tools


en:lesson7

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

en:lesson7 [2019/11/07 16:44]
golikov [Lesson 08. OrbiCraft positioning with magnetometer]
en:lesson7 [2020/03/25 16:28]
Line 1: Line 1:
-====== Lesson 07. OrbiCraft positioning with magnetometer ====== 
-===== Program for turning by magnetometer ===== 
-Create the program listed below. Factors for mag_calibrated function must be taken from Magneto software. 
- 
-{{::​07image001.png?​nolink&​400|}} 
- 
-Python code. 
- 
-<file python orientation.py>​ 
-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 = -50 # 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 
- 
-mtr_num = 1 # flywheel number 
-hyr_num = 1 # angular velocity sensor number 
-mag_num = 1 # magnetometer number 
- 
-# mag_calibrated function will adjust the magnetometer’s readings considering the calibration factors 
-def mag_calibrated(magx,​magy,​magz):​ 
- magx_cal = 1.06*(magx + -7.49) + -0.01*(magy + -23.59) + 0.07*(magz + -108.24) 
- magy_cal = -0.01*(magx + -7.49) + 1.11*(magy + -23.59) + 0.09*(magz + -108.24) 
- magz_cal = 0.07*(magx + -7.49) + 0.09*(magy + -23.59) + 1.00*(magz + -108.24) 
- return magx_cal, magy_cal, magz_cal 
-  
-# motor_new_speed_PD function defines flywheel’s 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)) 
- return mtr_new_speed 
- # return 0 
-  
-def initialize_all():​ # All systems’ initialization function 
- 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 the sensor on 
- sleep(1) 
-  
- print "​Enable magnetometer",​ mag_num 
- magnetometer_turn_on(mag_num) 
- sleep(1) 
-  
-def switch_off_all():​ # All systems’ switching off function 
- print "​Finishing..."​ 
- print "​Disable angular velocity sensor №", hyr_num 
- hyro_turn_off(hyr_num) ​  # Switch the sensor off 
- print "​Disable magnetometer",​ mag_num 
- magnetometer_turn_off(mag_num) 
- motor_set_speed(mtr_num,​ 0) 
- sleep (1) 
- motor_turn_off(mtr_num) 
- print "​Finish program"​ 
-  
-def control(): # Program’s main function 
- initialize_all() 
- mtr_state = 0 # Initialize flywheel status 
- hyro_state = 0 # Initialize angular velocity sensor status 
- mag_state = 0 # Initialize magnetometer status 
- alpha_goal = 0 # Target angle 
- omega_goal = 0 # Target angular velocity 
- mag_alpha = 0 
-  
- for i in range(500): 
- # sensors and flywheel data request 
- mag_state,​ magx_raw, magy_raw, magz_raw = magnetometer_request_raw(mag_num) 
- hyro_state,​ gx_raw, gy_raw, gz_raw = hyro_request_raw(hyr_num) ​ 
- mtr_state,​ mtr_speed = motor_request_speed(mtr_num) 
-  
- if not mag_state: # if magnetometer returned error code 0 (no error) 
- magx_cal,​ magy_cal, magz_cal = mag_calibrated(magx_raw,​magy_raw,​magz_raw) 
- magy_cal = - magy_cal # transition from the left coordinate system which is plotted on the magnetometer,​ to the right coordinate system to position the positive angle direction counter clock-wise ​ 
- mag_alpha = math.atan2(magy_cal,​ magx_cal)/​math.pi*180 
- print "​magx_cal =", magx_cal, "​magy_cal =", magy_cal, "​magz_cal =", magz_cal # output of the magnetometer calibrated values ​ 
- print "​mag_alpha atan2= ", mag_alpha 
- elif mag_state == 1: 
- print "Fail because of access error, check the connection"​ 
- elif mag_state == 2: 
- print "Fail because of interface error, check your code" 
-  
- if not hyro_state: # if the 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 # Выводим данные 
- 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,​mag_alpha,​alpha_goal,​gz_degs,​omega_goal) #​ setting the flywheel’s new speed  
- motor_set_speed(mtr_num,​ mtr_new_speed) 
-  
- sleep(time_step) 
- switch_off_all() 
-</​file>​ 
- 
-С code. 
-<file c orientation.c>​ 
-#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; /* 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)*/​ 
-  
-const int kp = -50; /* 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 
-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 
-// Maximum allowable flywheel speed, rpm 
-const int16_t mtr_max_speed = 5000; 
-int16_t mtr_new_speed;​ 
- 
-int mag_calibrated(int16_t *magx, int16_t *magy, int16_t *magz ){ 
-//​mag_calibrated function will adjust the magnetometer’s readings considering the calibration factors 
- float magx_cal; 
- magx_cal = 1.06*(*magx + -7.49) + -0.01*(*magy + -23.59) + 0.07*(*magz + -108.24); 
- float magy_cal; 
- magy_cal = -0.01*(*magx + -7.49) + 1.11*(*magy + -23.59) + 0.09*(*magz + -108.24); 
- float magz_cal; 
- magz_cal = 0.07*(*magx + -7.49) + 0.09*(*magy + -23.59) + 1.00*(*magz + -108.24); 
- *magx = (int16_t) magx_cal; 
- *magy = (int16_t) magy_cal; 
- *magz = (int16_t) magz_cal;  
- return 0; 
-} 
- 
-int motor_new_speed_PD(int mtr_speed, int omega, int omega_goal){  
- /* Function for calculating the new flywheel speed*/ 
-  
- mtr_new_speed = (int)(mtr_speed + 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);​ // Включаем ДУС 
- Sleep(1); 
- printf("​Enable magnetometer %d\n", mag_num); 
- magnetometer_turn_on(mag_num);​ 
- Sleep(1); 
-} 
- 
-int switch_off_all(){ 
-/* All systems’ switching off function*/ 
- printf("​Finishing..."​);​ 
- int16_t new_speed = mtr_new_speed; ​ 
- printf("​\nDisable angular velocity sensor №%d\n",​ hyr_num); 
- hyro_turn_off(hyr_num);​ 
- printf("​Disable magnetometer %d\n", mag_num); 
- magnetometer_turn_off(mag_num);​ 
- motor_set_speed(mtr_num,​ 0, &​new_speed);​ 
- Sleep (1); 
- motor_turn_off(mtr_num);​ 
- printf("​Finish program\n"​);​ 
- return 0; 
-} 
- 
-int control(){ 
- float mtr_new_speed;​ 
- //​magnetometer data 
- int16_t mgx_cal=0; 
- int16_t mgy_cal=0; 
- int16_t mgz_cal=0; 
- int16_t *magx_raw = &​mgx_cal;​ 
- int16_t *magy_raw = &​mgy_cal;​ 
- int16_t *magz_raw = &​mgz_cal;​ 
- //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;​ 
- int16_t mtr_speed; 
- int16_t omega; 
- int16_t omega_goal=0; ​ // Target angular speed 
-  
- initialize_all();​ 
-  
- int mtr_state = 0; // Initialize flywheel status 
- int hyro_state = 0; // Initialize angular velocity sensor status 
- int mag_state = 0; // Initialize magnetometer status 
-// int alpha_goal = 0; // Target angle  
- double mag_alpha = 0; 
- int i; 
- for (i = 0; i < 500; i++) { 
- mag_state = magnetometer_request_raw(mag_num,​ magx_raw, magy_raw, magz_raw); 
- 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);​ 
- if (!mag_state){ 
- mag_calibrated(magx_raw,​magy_raw,​magz_raw);​ 
- *magy_raw = - *magy_raw;/​* transition from the left coordinate system which is plotted on the magnetometer,​ to the right coordinate system to position the positive angle direction counter clock-wise*/​ 
- mag_alpha = atan2(mgy_cal,​ mgx_cal)/​M_PI*180;​ 
- printf("​magx_cal = %d", mgx_cal); 
- printf("​ magy_cal = %d", mgy_cal); 
- printf("​ magz_cal = %d", mgz_cal); 
- printf("​ mag_alpha atan2 = %f\n", mag_alpha); 
- } 
- else if (mag_state == 1){ 
- printf("​Fail because of access error, check the connection\n"​);​ 
-    } 
- else if (mag_state == 2){ 
- printf("​Fail because of interface error, check your code\n"​);​ 
-   } 
-  
- if (!hyro_state){ 
- float gx_degs = gx_raw * 0.00875; 
- float gy_degs = gy_raw * 0.00875; 
- float gz_degs = gz_raw * 0.00875; 
- /* if the angular velocity sensor sensor arranged via z-axis up, then satellite’s angular velocity matches to sensors’ readings of z-axis */ 
- omega = gz_degs; 
- printf("​\ngx_degs = %f", gx_degs); 
- printf("​ gy_degs = %f", gy_degs); 
- printf("​ gz_degs = %f\n", gz_degs); 
- } 
- else if (hyro_state == 1){ 
- printf("​Fail because of access error, check the connection\n"​);​ 
- } 
- else if (hyro_state == 2){ 
- printf("​Fail because of interface error, check your code\n"​);​ 
- } 
-  
- 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("​\nMotor_speed:​ %d\n", mtr_speed);​  
- // setting a new flywheel speed 
- mtr_new_speed = motor_new_speed_PD(mtr_speed,​omega,​omega_goal);​ 
- motor_set_speed(mtr_num,​ mtr_new_speed,​ &​omega);​ 
- }  
- Sleep(time_step);​  
- } 
- switch_off_all();​ 
- return 0; 
-} 
-</​file>​ 
- 
-===== Program performance analysis ​ ===== 
-To work with OrbiCraft ​ the program uses such functions: 
-<code python>​magnetometer_turn_on(num)</​code>​ – magnetometer switching’on function, where num is the number of the magnetometer. 
-<code python>​magnetometer_turn_off(num)</​code>​ – magnetometer switching’off function, where num is the number of the magnetometer. 
-<code python>​magnetometer_request_raw(num)</​code>​ – the function that returns raw data measured with magnetometer with num number presented as list with 4 digit values. 
-<code python>​hyro_turn_on(hyr_num)</​code>​ – angular velocity sensor’s switching’on function, where hyr_num is the sensor’s number. 
-<code python>​hyro_turn_off(hyr_num)</​code>​ – angular velocity sensor’s switching’off function, where hyr_num is the sensor’s number. 
-<code python>​hyro_request_raw(hyr_num)</​code>​ – the function that returns raw data measured with sensor with hyr_num number presented as list with 4 digit values. 
-<code python>​motor_turn_on(num)</​code>​ – flywheel’s ​ switching’on function, where num is flywheel number. 
-<code python>​motor_turn_off(num)</​code>​ – flywheel’s ​ switching’off function, where num is flywheel number. 
-<code python>​motor_set_speed(mtr_num,​ speed)</​code>​ – the function that sets flywheel’s (with mtr_num, number) speed to the speed value. 
- 
-===== Program’s test ===== 
- 
-Test the program at such values of alpha_goal target angle: 0, 90, -90, 180. 
- 
-At the turning at 180 degree target angle the program will perform incorrectly. 
- 
-To fix the functional bug add to the program angle_transformation function. 
- 
-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 (difference between the current angle and target angle alpha_goal). By default the angle will be defined in range from -180 to 180 degrees via atan2 funtion. 
- 
-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. 
- 
-<code python> 
-def angle_transformation(alpha,​ alpha_goal):​ 
- if alpha<​=(alpha_goal - 180): 
- alpha = alpha + 360 
- elif alpha>​(alpha_goal +180): 
- alpha = alpha - 360 
- return alpha 
-</​code>​ 
- 
-===== Corrected program’s test ===== 
- 
-Test the program at such values of alpha_goal target angle: 
-0, 90, -90, 180. 
-Now at the turning at 180 degree target angle the program will perform correct. 
- 
- 
- 
- 
  
en/lesson7.txt · Last modified: 2020/03/25 16:28 (external edit)