rotate.c

/***************************************************************************** 
 * 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);
}