Add sun sensor normals in the body frame of the spacecraft (#834)

src/adcs/imu.cpp

@@ -49,7 +49,7 @@ static void update_mag1(unsigned char mag1_mode, float mag_flt) {
   //   mag1.calibrate();
   // TODO :  ^^^^ --> Implement calibrate function
 
-  // Attempt a read if ready and ensure it was succesful
+  // Attempt a read if ready and ensure it was successful
   if (!mag1.is_functional()) return;
   if (!mag1.is_ready()) return;
   if (!mag1.read()) return;
@@ -60,7 +60,7 @@ static void update_mag1(unsigned char mag1_mode, float mag_flt) {
     utl::fp(mag1.get_b_y(), min_mag1_rd_mag, max_mag1_rd_mag),
     utl::fp(mag1.get_b_z(), min_mag1_rd_mag, max_mag1_rd_mag)
   };
-  data = mag1_to_body * data;
+  data = (mag1_to_body * data).eval();
 
   // Update the filtered magnetic field reading for magnetometer one
   mag1_rd = mag1_rd + (data - mag1_rd) * mag_flt;
@@ -93,7 +93,7 @@ static void update_mag2(unsigned char mag2_mode, float mag_flt) {
     utl::fp(mag2.get_b_y(), min_mag2_rd_mag, max_mag2_rd_mag),
     utl::fp(mag2.get_b_z(), min_mag2_rd_mag, max_mag2_rd_mag)
   };
-  data = mag2_to_body * data;
+  data = (mag2_to_body * data).eval();
 
   // Update the filtered magnetic field reading for magnetometer two
   mag2_rd = mag2_rd + (data - mag2_rd) * mag_flt;
@@ -135,7 +135,7 @@ static void update_gyr(float gyr_flt, float gyr_temp_target, float gyr_temp_flt,
     utl::fp(gyr.get_omega_y(), min_rd_omega, max_rd_omega),
     utl::fp(gyr.get_omega_z(), min_rd_omega, max_rd_omega)
   };
-  data = gyr_to_body * data;
+  data = (gyr_to_body * data).eval();
 
   // Read in temperature data and filter
   temp_data = utl::fp(gyr.get_temp(), min_rd_temp, max_rd_temp);

src/adcs/ssa.cpp

@@ -10,9 +10,6 @@
 // Cornell Univeristy
 //
 
-// TODO : Fill in the sun sensor normal vectors
-// TODO : Consider pulling the algorithm from PSim
-
 #ifdef SSA_LOG_LEVEL
   #undef LOG_LEVEL
   #define LOG_LEVEL SSA_LOG_LEVEL
@@ -117,26 +114,31 @@ void update_sensors(float adc_flt) {
   LOG_TRACE_printlnF("Complete")
 }
 
-static lin::Matrix<float, 0, 3, 20, 3> A, Q;
-static lin::Vector<float, 0, 20> b;
-static lin::Matrix<float, 3, 3> R;
-static lin::Vector3f x;
-
 unsigned char calculate_sun_vector(lin::Vector3f &sun_vec) {
-  // Prepare least squares problem
-  std::size_t j = 0;
-  for (std::size_t i = 0; i < voltages.size(); i++) { // TODO : Only include is_functional ADCs
-    if (voltages(i) > sensor_voltage_thresh * lin::norm(lin::row(normals, i))) {
-      lin::row(A, j) = lin::row(normals, i);
-      b(j) = voltages(i);
-      j++;
+  lin::Matrixf<0, 3, 20, 3> A, Q;
+  lin::Vectorf<0, 20> b;
+  lin::Matrix<float, 3, 3> R;
+  lin::Vector3f x;
+
+  // Prepare the least squares problem
+  lin::size_t k = 0;
+  for (lin::size_t i = 0; i < 5; i++) {
+    // Skip ADCs that aren't functional
+    if (!adcs[i].is_functional()) continue;
+
+    for (lin::size_t j = 4 * i; j < 4 * i + 4; j++) {
+      if (voltages(j) > sensor_voltage_thresh) {
+        lin::row(A, k) = lin::row(normals, j);
+        b(k) = voltages(j);
+        k++;
+      }
     }
   }
   // Ensure system is overdefined
-  if (j < sensor_count_thresh) return SSAMode::SSA_FAILURE;
+  if (k < sensor_count_thresh) return SSAMode::SSA_FAILURE;
   // Calculate sun vector
-  b.resize(j, 1); // Hacky resize call
-  A.resize(j, 3);
+  b.resize(k, 1); // Hacky resize call
+  A.resize(k, 3);
   lin::qr(A, Q, R);
   lin::backward_sub(R, x, (lin::transpose(Q) * b).eval());
   // Return sun vector

src/adcs/ssa_config.hpp

@@ -10,7 +10,6 @@
 // Cornell Univeristy
 //
 
-// TODO : Fill in the normal vector matrices
 // TODO : Consider moving this definition to the psim repository
 
 #ifndef SRC_ADCS_SSA_CONFIG_HPP_
@@ -60,71 +59,39 @@ static unsigned long const adcx_timeout = 10000;
 /** Required number of sensors in view of the sun to calculate the sun vector
  *  via least squares. */
 static unsigned int const sensor_count_thresh = 4;
-/** Relative voltage threshold for a sun sensor to be considered in view of the
- *  sun. The actual voltage cutoff is the magnitude of the normal vector times
- *  this value. */
-static float const sensor_voltage_thresh = 0.5f;
+/** Voltage threshold for a sun sensor to be considered in view of the sun. */
+static float const sensor_voltage_thresh = 1.0f;
 
+static constexpr float s20 = std::cos(20.0);
+static constexpr float c20 = std::sin(20.0);
 /** Matrix containing the normal vectors for all sun sensors in the ADCS system.
  *  Should be defined for the leader and follower spacecraft individually. */
 static const lin::Matrix<float, 20, 3> normals({
-#ifdef PAN_LEADER
-
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f
-
-#elif PAN_FOLLOWER
-
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f,
-  0.0f, 0.0f, 0.0f
-
-#else
-static_assert(false, "Must define PAN_LEADER or PAN_FOLLOWER");
-#endif
+  // Face 2 (+x)
+   c20,  0.f,  s20,
+   c20,  0.f, -s20,
+   c20, -s20,  0.f,
+   c20,  s20,  0.f,
+  // Face 3 (-y)
+  -s20, -c20,  0.f,
+   s20, -c20,  0.f,
+   0.f, -c20, -s20,
+   0.f, -c20,  s20,
+  // Face 4 (-x)
+  -c20,  s20,  0.f,
+  -c20, -s20,  0.f,
+  -c20,  0.f, -s20,
+  -c20,  0.f,  s20,
+  // Face 5 (+y)
+   s20,  c20,  0.f,
+  -s20,  c20,  0.f,
+   0.f,  c20, -s20,
+   0.f,  c20,  s20,
+  // Face 6 (+z)
+  -s20,  0.f,  c20,
+   s20,  0.f,  c20,
+   0.f, -s20,  c20,
+   0.f,  s20,  c20
 });
 }  // namespace ssa
 }  // namespace adcs