Attitude Estimator Control Task (#91)

* attitude estimator only commit actually

* add to main control loop

* undefined ref error and fixes

* estimator test

* pass serializer test

* fix include

* pass teensy build

* address some changes

* change serializer bounds, pre const

* add readablefields to constructor list

* pan epoch, readable or internal T_T

* piksi fix, but time assert wrong

* fix const, but assertions still failing

* change mag to vector

* finally fix asserting statefields

* pan epoch from psim

lib/Drivers/Piksi.cpp

@@ -99,6 +99,7 @@ void Piksi::disable() {
 void Piksi::get_gps_time(msg_gps_time_t *time) {
     time->wn = _gps_time.wn;
     time->tow = _gps_time.tow;
+    time->ns = _gps_time.ns;
 }
 
 unsigned int Piksi::get_dops_tow() { return _dops.tow; }

lib/Drivers/Piksi.hpp

@@ -386,7 +386,7 @@ class Piksi {
     msg_log_t *_latest_log;                 // Pointer to the latest log in the list
 
     // Piksi data containers.
-    msg_gps_time_t _gps_time;
+    msg_gps_time_t _gps_time = {};
     msg_dops_t _dops;
     msg_pos_ecef_t _pos_ecef;
     msg_baseline_ecef_t _baseline_ecef;

src/FCCode/AttitudeEstimator.cpp

@@ -0,0 +1,81 @@
+#include "AttitudeEstimator.hpp"
+#include <gnc_constants.hpp>
+
+AttitudeEstimator::AttitudeEstimator(StateFieldRegistry &registry,
+    unsigned int offset) 
+    : TimedControlTask<void>(registry, offset),
+    pan_epoch(gnc::constant::init_gps_week_number,
+        gnc::constant::init_gps_time_of_week,
+        gnc::constant::init_gps_nanoseconds),
+    data(),
+    state(),
+    estimate(),
+    q_body_eci_sr(),
+    q_body_eci_f("attitude_estimator.q_body_eci", q_body_eci_sr),
+    w_body_sr(-55, 55, 32*3),
+    w_body_f("attitude_estimator.w_body", w_body_sr)
+    {
+
+        piksi_time_fp = find_readable_field<gps_time_t>("piksi.time", __FILE__, __LINE__),
+        pos_vec_ecef_fp = find_readable_field<d_vector_t>("piksi.pos", __FILE__, __LINE__),
+        ssa_vec_rd_fp = find_readable_field<f_vector_t>("adcs_box.sun_vec", __FILE__, __LINE__),
+        mag_vec_fp = find_readable_field<f_vector_t>("adcs_box.mag_vec", __FILE__, __LINE__),
+
+        //assert inputs are found
+        assert(piksi_time_fp);
+        assert(pos_vec_ecef_fp);
+        assert(ssa_vec_rd_fp);
+        assert(mag_vec_fp);
+
+        //Add outputs
+        add_readable_field(q_body_eci_f);
+        add_readable_field(w_body_f);
+    }
+
+void AttitudeEstimator::execute(){
+    set_data();
+    gnc::estimate_attitude(state, data, estimate);
+    set_estimate();
+}
+
+void AttitudeEstimator::set_data(){
+    data.t = (double)(((unsigned long)(piksi_time_fp->get() - pan_epoch))/(1e9L));
+
+    data.r_ecef = lin::Vector3d({
+        pos_vec_ecef_fp->get()[0], 
+        pos_vec_ecef_fp->get()[1], 
+        pos_vec_ecef_fp->get()[2]
+    });
+
+    data.b_body = lin::Vector3f({
+        mag_vec_fp->get()[0],
+        mag_vec_fp->get()[1],
+        mag_vec_fp->get()[2]
+    });
+
+    data.s_body = lin::Vector3f({
+        ssa_vec_rd_fp->get()[0],
+        ssa_vec_rd_fp->get()[1],
+        ssa_vec_rd_fp->get()[2],
+    });
+
+}
+
+void AttitudeEstimator::set_estimate(){
+
+    f_quat_t q_temp;
+    q_temp[0] = estimate.q_body_eci.operator()(0);
+    q_temp[1] = estimate.q_body_eci.operator()(1);
+    q_temp[2] = estimate.q_body_eci.operator()(2);
+    q_temp[3] = estimate.q_body_eci.operator()(3);
+    q_body_eci_f.set(q_temp);
+
+    f_vector_t w_temp;
+    w_temp[0] = estimate.w_body.operator()(0);
+    w_temp[1] = estimate.w_body.operator()(1);
+    w_temp[2] = estimate.w_body.operator()(2);  
+    w_body_f.set(w_temp);
+
+}
+
+

src/FCCode/AttitudeEstimator.hpp

@@ -0,0 +1,67 @@
+#ifndef ATTITUDE_ESTIMATOR_HPP_
+#define ATTITUDE_ESTIMATOR_HPP_
+
+#include <gnc_attitude_estimation.hpp>
+#include <TimedControlTask.hpp>
+
+/**
+ * @brief Using raw sensor inputs, determine the attitude and angular state
+ * of the spacecraft.
+ */
+class AttitudeEstimator : public TimedControlTask<void> {
+   public:
+    /**
+     * @brief Construct a new Attitude Estimator.
+     * 
+     * @param registry 
+     */
+    AttitudeEstimator(StateFieldRegistry& registry, unsigned int offset);
+
+    /**
+     * @brief Using raw sensor inputs, determine the attitude and angular state
+     * of the spacecraft.
+     */
+    void execute() override;
+
+   protected:
+
+    /**
+     * @brief Read data from field pointers and set the data object
+     * 
+     */
+    void set_data();
+
+    /**
+     * @brief Set the estimate object from the outputs of the gnc estimate
+     * 
+     */
+    void set_estimate();
+
+    const gps_time_t pan_epoch;
+
+    /**
+     * @brief Inputs collected from Piksi and ADCSBoxMonitor.
+     */
+    //! Time from Piksi
+    const ReadableStateField<gps_time_t>* piksi_time_fp;
+    //! Position of this satellite, Vector of doubles
+    const ReadableStateField<d_vector_t>* pos_vec_ecef_fp;
+    //! Sun vector of this satellite, in the body frame.
+    const ReadableStateField<f_vector_t>* ssa_vec_rd_fp;
+    //! Magnetic field vector of this satellite in the body frame.
+    const ReadableStateField<f_vector_t>* mag_vec_fp;
+
+    //kyle's gnc structs
+    gnc::AttitudeEstimatorData data;
+    gnc::AttitudeEstimatorState state;
+    gnc::AttitudeEstimate estimate;
+
+    //AttitudeEstimate
+    Serializer<f_quat_t> q_body_eci_sr;
+    ReadableStateField<f_quat_t> q_body_eci_f;
+    Serializer<f_vector_t> w_body_sr;
+    ReadableStateField<f_vector_t> w_body_f;
+
+};
+
+#endif

src/FCCode/FieldCreatorTask.hpp

@@ -17,6 +17,9 @@ class FieldCreatorTask : public ControlTask<void> {
       WritableStateField<float> adcs_min_stable_ang_rate_f;
       WritableStateField<bool> docking_config_cmd_f;
 
+      ReadableStateField<f_vector_t> ssa_vec_rd_f;
+      ReadableStateField<f_vector_t> mag_vec_f;
+
       InternalStateField<unsigned int> snapshot_size_f;
       InternalStateField<char*> radio_mo_packet_f;
       InternalStateField<char*> radio_mt_packet_f;
@@ -30,6 +33,8 @@ class FieldCreatorTask : public ControlTask<void> {
         adcs_ang_rate_f("adcs.ang_rate", Serializer<float>(0, 10, 4)),
         adcs_min_stable_ang_rate_f("adcs.min_stable_ang_rate", Serializer<float>(0, 10, 4)),
         docking_config_cmd_f("docksys.config_cmd", Serializer<bool>()),
+        ssa_vec_rd_f("adcs_box.sun_vec", Serializer<f_vector_t>(0,1,32*3)),
+        mag_vec_f("adcs_box.mag_vec", Serializer<f_vector_t>(0,1,32*3)),
         snapshot_size_f("downlink_producer.snap_size"),
         radio_mo_packet_f("downlink_producer.mo_ptr"),
         radio_mt_packet_f("uplink_consumer.mt_ptr"),
@@ -47,6 +52,10 @@ class FieldCreatorTask : public ControlTask<void> {
           // For DockingController
           add_writable_field(docking_config_cmd_f);
 
+          // For AttitudeEstimator
+          add_readable_field(ssa_vec_rd_f);
+          add_readable_field(mag_vec_f);
+
           // For QuakeManager
           add_internal_field(snapshot_size_f);
           add_internal_field(radio_mo_packet_f);

src/FCCode/MainControlLoop.cpp

@@ -26,6 +26,7 @@ MainControlLoop::MainControlLoop(StateFieldRegistry& registry)
       debug_task(registry, debug_task_offset),
       PIKSI_INITIALIZATION,
       piksi_control_task(registry, piksi_control_task_offset, piksi),
+      attitude_estimator(registry, attitude_estimator_offset),
       gomspace(&hk, &config, &config2),
       gomspace_controller(registry, gomspace_controller_offset, gomspace),
       mission_manager(registry, mission_manager_offset),

src/FCCode/MainControlLoop.hpp

@@ -7,6 +7,7 @@
 
 #include "ClockManager.hpp"
 #include "PiksiControlTask.hpp"
+#include "AttitudeEstimator.hpp"
 #include "GomspaceController.hpp"
 #include "DebugTask.hpp"
 #include "FieldCreatorTask.hpp"
@@ -26,6 +27,7 @@ class MainControlLoop : public ControlTask<void> {
 
     Devices::Piksi piksi;
     PiksiControlTask piksi_control_task;
+    AttitudeEstimator attitude_estimator;
 
     Devices::Gomspace::eps_hk_t hk;
     Devices::Gomspace::eps_config_t config; 
@@ -44,13 +46,15 @@ class MainControlLoop : public ControlTask<void> {
     #ifdef HOOTL
         static constexpr unsigned int debug_task_offset          =   5500;
         static constexpr unsigned int piksi_control_task_offset  =  55000;
+        static constexpr unsigned int attitude_estimator_offset  =  85500;
         static constexpr unsigned int gomspace_controller_offset = 106500;
         static constexpr unsigned int mission_manager_offset     = 111600;
         static constexpr unsigned int docking_controller_offset  = 152400;
         static constexpr unsigned int quake_manager_offset       = 153400;
     #else
         static constexpr unsigned int debug_task_offset          =   5500;
         static constexpr unsigned int piksi_control_task_offset  =   6000;
+        static constexpr unsigned int attitude_estimator_offset  =  85500;
         static constexpr unsigned int gomspace_controller_offset =  57500;
         static constexpr unsigned int mission_manager_offset     =  62600;
         static constexpr unsigned int docking_controller_offset  = 103400;

test/test_fsw_attitude_estimator/test_attitude_estimator.cpp

@@ -0,0 +1,75 @@
+#include "../StateFieldRegistryMock.hpp"
+
+#include "../../src/FCCode/AttitudeEstimator.hpp"
+#include <unity.h>
+
+class TestFixture {
+    public:
+        StateFieldRegistryMock registry;
+
+        // pointers to input statefields
+        std::shared_ptr<ReadableStateField<gps_time_t>> piksi_time_fp;
+        std::shared_ptr<ReadableStateField<d_vector_t>> pos_vec_ecef_fp;
+        std::shared_ptr<ReadableStateField<f_vector_t>> ssa_vec_rd_fp;
+        std::shared_ptr<ReadableStateField<f_vector_t>> mag_vec_fp;
+
+        // pointers to output statefields for easy access
+        ReadableStateField<f_quat_t>* q_body_eci_fp;
+        ReadableStateField<f_vector_t>* w_body_fp;
+
+        std::unique_ptr<AttitudeEstimator> attitude_estimator;
+
+        // Create a TestFixture instance of AttitudeEstimator with pointers to statefields
+        TestFixture() : registry(){
+
+                //create input statefields
+                piksi_time_fp = registry.create_readable_field<gps_time_t>("piksi.time");
+                pos_vec_ecef_fp = registry.create_readable_vector_field<double>("piksi.pos",0.0L,1000000.0L,64*3);
+                ssa_vec_rd_fp = registry.create_readable_vector_field<float>("adcs_box.sun_vec",-1.0,1.0,32*3),
+                mag_vec_fp = registry.create_readable_vector_field<float>("adcs_box.mag_vec",-16e-4,16e4,32*3),
+
+                attitude_estimator = std::make_unique<AttitudeEstimator>(registry, 0);  
+
+                // initialize pointers to statefields
+                q_body_eci_fp = registry.find_readable_field_t<f_quat_t>("attitude_estimator.q_body_eci");
+                w_body_fp = registry.find_readable_field_t<f_vector_t>("attitude_estimator.w_body");
+
+                assert(q_body_eci_fp);
+                assert(w_body_fp);
+
+        }
+};
+
+void test_task_initialization()
+{
+        TestFixture tf;
+}
+
+void test_execute(){
+
+}
+
+int test_control_task()
+{
+        UNITY_BEGIN();
+        RUN_TEST(test_task_initialization);
+        RUN_TEST(test_execute);
+        return UNITY_END();
+}
+
+#ifdef DESKTOP
+int main()
+{
+        return test_control_task();
+}
+#else
+#include <Arduino.h>
+void setup()
+{
+        delay(2000);
+        Serial.begin(9600);
+        test_control_task();
+}
+
+void loop() {}
+#endif

test/test_fsw_piksi_control_task/test_piksi_control_task.cpp

@@ -4,6 +4,8 @@
 #include <unity.h>
 #include <Piksi.hpp>
 
+#include <iostream>
+
 #include "../../src/FCCode/piksi_mode_t.enum"
 
 #define assert_piksi_mode(x) {\