-
Notifications
You must be signed in to change notification settings - Fork 0
/
rotate.c
126 lines (106 loc) · 7.63 KB
/
rotate.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
/*****************************************************************************
* This function reads the output position quaternion from position.txt *
* generated by triad.c, the Earth’s magnetic field from IGRF, and the *
* current velocity from the RamSat telemetry. It finds the error between *
* the position quaternion and the desired quaternion and the error between *
* the current angular velocity and the desired angular velocity. It also *
* calculates the inertia matrix for the RamSat. Finally, it outputs a the *
* magnetic dipole needed to point the satellite RamSat toward the Earth’s *
* magnetic field over the settling time chosen. *
* *
* Code is written by Melissa Allen-Dumas based on lessons posted by Glenn *
* Fiedler in “Physics in 3D,” a Gaffer On Games article located at: *
* gafferongames.com/post/physics_in_3d *
*****************************************************************************/
//#include "xc.h”
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#define PI 3.141592653589793238462643
float qposition[4]; // from triad.c
float desquaternion[4] = {0, 0, 0, 1}; // desired quaternion; Z-axis points toward Earth
float desqconj[4] = {0, 0, 0, -1}; // conjugate of desired quaternion to compute
// quaternion error
float bfield[3]; // Earth’s magnetic field, B
float omega[3]; // current angular velocity [rad/s]
float desomega[3] = {0.0225, 0.0225, 0.0225}; // desired angular velocity [rad/s]
float ts[3] = {30, 30, 30}; // settling time in seconds
float zeta[3] = {0.65, 0.65, 0.65}; // damping coefficient
float earthradius = 6378000; // in meters
float satheight = 400000; // in meters
float maxtorque = 5.2E-6; // in Newton meters
int main(void)
{
// Normalize the position quaternion
float qmagnitude = sqrt(qposition[0]*qposition[0]+qposition[1]*qposition[1]+qposition[2]*qposition[2]+qposition[3]*qposition[3]);
float qnorm[4] = {qposition[0]/qmagnitude, qposition[1]/qmagnitude, qposition[2]/qmagnitude, qposition[3]/qmagnitude};
// Normalize the Earth’s magnetic field vector
float bmagnitude = sqrt(bfield[0]*bfield[0]+bfield[1]*bfield[1]+bfield[2]*bfield[2]);
float bnorm[3] = {bfield[0]/bmagnitude, bfield[1]/bmagnitude, bfield[2]/bmagnitude};
/**************************************************************************************
* Here we define the inertia and its inverse by which the angular momentum will be *
* multiplied to get angular velocity, which forms the vector portion of the torque *
* quaternion. Values for ibody are hard coded here. Units are kg for mass and m *
* for length dimensions. Z is the longitudinal axis. Formula for rectangular rigid *
* body inertia accessed at: https://en.wikipedia.org/wiki/List_of_moments_of_inertia *
**************************************************************************************/
// Declare RamSat dimensions
float satmass = 2.556; // units: kg
float satheight = 0.20; // units: m
float satwidth = 0.10;
float satdepth = 0.10;
// Calculate the inertia matrix for the satellite body (This is the J term in torque calc)
float ibody[3][3] = { {(satmass*((satheight*satheight)+(satdepth*satdepth)))/12, 0, 0},
{ 0, (satmass*((satwidth*satwidth)+(satheight*satheight)))/12, 0},
{ 0, 0, (satmass*((satdepth*satdepth)+(satwidth*satwidth)))/12} };
// Calculate the determinant of the body inertia matrix
float determinant = (ibody[0][0] * (ibody[1][1]*ibody[2][2]-ibody[1][2]*ibody[2][1]))
- (ibody[0][1] * (ibody[0][1]*ibody[2][2]-ibody[2][1]*ibody[0][2]))
+ (ibody[0][2] * (ibody[1][0]*ibody[2][1]-ibody[1][1]*ibody[2][1]));
// Calculate the inverse of the body inertia matrix
float ibodyinv[3][3] = { {((ibody[1][1]*ibody[2][2]) - (ibody[2][1]*ibody[1][2]))/determinant,
-((ibody[0][1]*ibody[2][2]) - (ibody[2][1]*ibody[0][2]))/determinant,
((ibody[0][1]*ibody[1][2])-(ibody[1][1]*ibody[0][2]))/determinant},
{-((ibody[1][0]*ibody[2][2]) - (ibody[2][0]*ibody[1][2]))/determinant,
((ibody[0][0]*ibody[2][2]) - (ibody[2][0]*ibody[0][2]))/determinant,
-((ibody[0][0]*ibody[1][2])-(ibody[1][0]*ibody[0][2]))/determinant},
{((ibody[1][0]*ibody[2][1]) - (ibody[2][0]*ibody[1][1]))/determinant,
-((ibody[0][0]*ibody[2][1]) - (ibody[2][0]*ibody[0][1]))/determinant,
((ibody[0][0]*ibody[1][1])-(ibody[1][0]*ibody[0][1]))/determinant} };
/**************************************************************************************
* Here we calculate the control (or required) torque for responding to forces in the *
* space environment. This value informs the RamSat magnetic dipole calculations. *
* Formulation for this Proportional-Derivative magnetic control law is taken from: *
* https://digitalcommons.calpoly.edu/cgi/viewcontent.cgi?article=3266&context=theses *
**************************************************************************************/
// natural frequency
float omegan[3] = {4.4/(ts[0]*zeta[0]), 4.4/(ts[1]*zeta[1]), 4.4/(ts[2]*zeta[2])};
// proportional gains (2*J*zeta*omegan)
float kp[3] = {2*ibody[0][0]*zeta[0]*omegan[0],
2*ibody[1][1]*zeta[1]*omegan[1],
2*ibody[2][2]*zeta[2]*omegan[2]};
// derivative gains (2*J*omegan^2)
float kd[3] = {2*ibody[0][0]*omegan[0]*omegan[0],
2*ibody[1][1]*omegan[1]*omegan[1],
2*ibody[2][2]*omegan[2]*omegan[2]};
// attitude error (position quaternion vs desired position quaternion)
// quaternion multiply function from http://www.cs.cmu.edu/~baraff/sigcourse/notesd1.pdf
float ae[4] = {desqconj[0]*qnorm[0]-(desqconj[1]*qnorm[1]+desqconj[2]*qnorm[2]+desqconj[3]*qnorm[3]),
desqconj[0]*qnorm[1]+qnorm[0]*desqconj[1]+desqconj[2]*qnorm[3]-desqconj[3]*qnorm[2],
desqconj[0]*qnorm[2]+qnorm[0]*desqconj[2]+desqconj[3]*qnorm[1]-desqconj[1]*qnorm[3],
desqconj[0]*qnorm[3]+qnorm[0]*desqconj[3]+desqconj[1]*qnorm[2]-desqconj[2]*qnorm[1]};
// angular velocity error (actual angular velocity minus desired angular velocity )
float omegae[3] = {omega[0]-desomega[0], omega[1]-desomega[1], omega[2]-desomega[2]};
// control torque (Nm) calculation
// (Need to check the first term. Multiplied the vector part of the quaternion with kp)
float propterm[3] = {-(kp[0]*ae[1]), -(kp[1]*ae[2]), -(kp[2]*ae[3])};
float derivterm[3] = {kd[0]*omegae[0], kd[1]*omegae[1], kd[2]*omegae[2]};
float torque[3] = {propterm[0]-derivterm[0], propterm[1]-derivterm[1], propterm[2]-derivterm[2]};
// Calculate the magnetic dipole (Am^2) required to rotate the RamSat body to the desired attitude
// bnorm X torque
float dipole[3] = { (bnorm[1]*torque[2]-bnorm[2]*torque[1]),
(bnorm[2]*torque[0]-bnorm[0]*torque[2]),
(bnorm[0]*torque[1]-bnorm[1]*torque[0]) };
exit(0);
}