Merge pull request #60 from RIT-Launch-Initiative/feature/Aaron/Event…

…Detection

Event Detection (Liftoff, Apogee, Landing)

event/ApogeeEvent.h

@@ -0,0 +1,56 @@
+/*******************************************************************************
+*
+*  Name: ApogeeEvent.h
+*
+*  Purpose: Handle detecting an apogee event
+*
+*  Author: Aaron Chan
+*
+*  RIT Launch Initiative
+*
+*******************************************************************************/
+
+#ifndef LAUNCH_CORE_APOGEEEVENT_H
+#define LAUNCH_CORE_APOGEEEVENT_H
+
+#include <stdint.h>
+
+#include "return.h"
+#include "event/Event.h"
+#include "return.h"
+#include "sched.h"
+#include "sched/macros.h"
+
+class ApogeeEvent : public Event {
+public:
+    ApogeeEvent(bool *const p_apogee_event_detected, uint16_t *const p_current_altitude) : Event(p_apogee_event_detected), m_highest_altitude(*p_current_altitude), p_current_altitude(p_current_altitude) {}
+
+    RetType calculate_event() override {
+        RESUME();
+
+        RetType ret = RET_SUCCESS;
+        if (*p_current_altitude > m_highest_altitude) {
+            m_highest_altitude = *p_current_altitude;
+            m_count++;
+        } else if (*p_current_altitude < m_highest_altitude) {
+            m_count++;
+
+            if (m_count >= DETECT_COUNT) {
+                *p_event_detected = true;
+                call_hooks();
+            }
+        } else {
+            m_count = 0;
+        }
+
+        RESET();
+        return ret;
+    };
+
+private:
+    uint16_t const *p_current_altitude;
+    uint16_t m_highest_altitude = 0;
+    uint8_t m_count = 0;
+};
+
+#endif //LAUNCH_CORE_APOGEEEVENT_H

event/BoostEvent.h

@@ -0,0 +1,65 @@
+/*******************************************************************************
+*
+*  Name: event_detection.h
+*
+*  Purpose: Handles detecting a boost event
+*
+*  Author: Aaron Chan
+*
+*  RIT Launch Initiative
+*
+*******************************************************************************/
+
+#ifndef LAUNCH_CORE_BOOSTEVENT_H
+#define LAUNCH_CORE_BOOSTEVENT_H
+
+#include <stdint.h>
+
+#include "return.h"
+#include "event/Event.h"
+#include "return.h"
+#include "sched.h"
+#include "sched/macros.h"
+
+
+class BoostEvent : public Event {
+public:
+    BoostEvent(bool *const p_boost_event_detected, int16_t *const p_current_accel, int16_t *const p_current_altitude) : Event(p_boost_event_detected),
+                                                                                                                        m_avg_accel(*p_current_accel), m_avg_altitude(*p_current_altitude),
+                                                                                                                        p_current_accel(p_current_accel), p_current_altitude(p_current_altitude),
+                                                                                                                        m_count(0) {};
+
+    RetType calculate_event() override {
+        RESUME();
+
+        RetType ret = RET_SUCCESS;
+
+        if (*p_current_accel > m_avg_accel && *p_current_altitude > m_avg_altitude) {
+            m_avg_accel = (m_avg_accel + *p_current_accel) / 2;
+            m_avg_altitude = (m_avg_altitude + *p_current_altitude) / 2;
+            m_count++;
+
+            if (m_count >= DETECT_COUNT) {
+                *p_event_detected = true;
+
+                call_hooks();
+            }
+        } else {
+            m_count = 0;
+        }
+
+        RESET();
+        return ret;
+    };
+
+private:
+    int16_t m_avg_accel;
+    int16_t m_avg_altitude;
+
+    int16_t *const p_current_accel;
+    int16_t *const p_current_altitude;
+
+    uint8_t m_count{};
+};
+
+#endif //LAUNCH_CORE_BOOSTEVENT_H

event/Event.h

@@ -0,0 +1,80 @@
+/*******************************************************************************
+*
+*  Name: Event.h
+*
+*  Purpose: Interface for defining event detection functions
+*
+*  Author: Aaron Chan
+*
+*  RIT Launch Initiative
+*
+*******************************************************************************/
+
+#ifndef LAUNCH_CORE_EVENT_H
+#define LAUNCH_CORE_EVENT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "return.h"
+#include "sched/sched.h"
+
+
+class Event {
+public:
+    const uint8_t NUM_HOOKS = 5;
+    const uint8_t DETECT_COUNT = 5;
+
+    Event(bool* const p_event_detected) : num_callbacks(0), p_event_detected(p_event_detected) {}
+
+    virtual ~Event() = default;
+
+    /**
+     * @brief Task for calculating the event and updating the event detected flag
+     *
+     * @return
+     */
+    virtual RetType calculate_event() = 0;
+
+    /**
+     * @brief Get the pointer to the event detected flag
+     *
+     *
+     * @return
+     */
+    [[nodiscard]] bool const *get_event_status() const {
+        return p_event_detected;
+    };
+
+    /**
+     * @brief Adds a hook to the event
+     *
+     * @param hook
+     * @return
+     */
+    constexpr bool register_callback(RetType (*hook)(void *args)) {
+        if (num_callbacks >= NUM_HOOKS) return false;
+
+        callbacks[num_callbacks++] = hook;
+        return true;
+    };
+
+private:
+    RetType (*callbacks[5])(void *args);
+    void *callback_args[5];
+
+    uint8_t num_callbacks;
+
+protected:
+    bool *const p_event_detected;
+
+    /**
+     * @brief Calls all hooks for the event
+     */
+    void call_hooks() {
+        for (uint8_t i = 0; i < num_callbacks; i++) {
+             sched_start(callbacks[i], callback_args[i]);
+        }
+    }
+};
+
+#endif //LAUNCH_CORE_EVENT_H

event/LandingEvent.h

@@ -0,0 +1,73 @@
+/*******************************************************************************
+*
+*  Name: LandingEvent.h
+*
+*  Purpose: Handle detecting a landing event
+*
+*  Author: Aaron Chan
+*
+*  RIT Launch Initiative
+*
+*******************************************************************************/
+
+#ifndef LAUNCH_CORE_LANDINGEVENT_H
+#define LAUNCH_CORE_LANDINGEVENT_H
+
+#include <stdint.h>
+#include <math.h>
+
+#include "return.h"
+#include "event/Event.h"
+#include "return.h"
+#include "sched.h"
+#include "sched/macros.h"
+
+class LandingEvent : public Event {
+public:
+    LandingEvent(bool *const p_landing_event_detected, bool *const p_past_event, int16_t *const p_current_accel, int16_t *const p_current_altitude) : Event(p_landing_event_detected), m_avg_altitude(*p_current_altitude),
+                                                                                                                                                      p_current_altitude(p_current_altitude), p_current_accel(p_current_accel),
+                                                                                                                                                      p_past_event(p_past_event) {}
+
+    RetType calculate_event() override {
+        RESUME();
+
+        RetType ret = RET_SUCCESS;
+        if (*p_past_event && is_accel_in_range() &&  is_altitude_in_range()) {
+            m_count++;
+
+            m_avg_altitude = (m_avg_altitude + *p_current_altitude) / 2;
+
+            if (m_count >= DETECT_COUNT) {
+                *p_event_detected = true;
+                call_hooks();
+            }
+        } else {
+            m_count = 0;
+        }
+
+        RESET();
+        return ret;
+    };
+
+private:
+    int16_t m_avg_altitude{};
+    int16_t *const p_current_altitude;
+    int16_t *const p_current_accel;
+    uint8_t m_count = 0;
+    bool *const p_past_event;
+
+    static constexpr uint8_t THREE_G_ACCEL = 9.81 * 3;
+
+    [[nodiscard]] bool is_accel_in_range() const {
+        return *p_current_accel < THREE_G_ACCEL;
+    }
+
+    [[nodiscard]] bool is_altitude_in_range() const {
+        const uint8_t threshold_range = 50;
+        const int16_t altitude_diff = *p_current_altitude - m_avg_altitude;
+
+        return altitude_diff < threshold_range;
+    }
+};
+
+#endif //LAUNCH_CORE_LANDINGEVENT_H