diff --git a/src/boards/Homeone/ADA/ADA.cpp b/src/boards/Homeone/ADA/ADA.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9db38b6896257056f224a5a2d7f93ae0458d6af3 --- /dev/null +++ b/src/boards/Homeone/ADA/ADA.cpp @@ -0,0 +1,353 @@ +/* Copyright (c) 2018 Skyward Experimental Rocketry + * Authors: Luca Mozzarelli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <boards/Homeone/ADA/ADA.h> +#include <boards/Homeone/ADA/ADA_config.h> + +#include <events/EventBroker.h> + +#include <boards/Homeone/Events.h> +#include <boards/Homeone/Topics.h> + +#include "Debug.h" + +namespace HomeoneBoard +{ +namespace FMM +{ + +/* --- LIFE CYCLE --- */ +ADA::ADA() + : FSM(&ADA::stateCalibrating), + filter(Matrix{3, 3, P_data}, Matrix{1, 1, R_data}, Matrix{3, 3, Q_data}, + Matrix{1, 3, (float[]){1, 0, 0}}) +{ + // Subscribe to topics + sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); + sEventBroker->subscribe(this, TOPIC_TC); + sEventBroker->subscribe(this, TOPIC_ADA); + + // Set state propagation matrix + // Note that sampling frequency is supposed to be constant and known at + // compile time. If this is not the case the matrix has to be updated at + // each iteration + + // clang-format off + float Phi_data[9] = + { 1, samplingPeriod, 0.5 * samplingPeriod * samplingPeriod, + 0, 1, samplingPeriod, + 0, 0, 1 + }; + // clang-format on + + filter.Phi.set(Phi_data); +} + +/* --- INSTANCE METHODS --- */ + +void ADA::update(float pressure) +{ + switch (status.state) + { + case ADAState::CALIBRATING: + { + // Calibrating state: update calibration data + + // Save old avg to compute var + float old_avg = calibrationData.avg; + + // Update avg + calibrationData.avg = (calibrationData.avg * calibrationData.n_samples + pressure) / (calibrationData.n_samples + 1); + calibrationData.n_samples = calibrationData.n_samples + 1; + + // Update var + float S_1 = calibrationData.var*(calibrationData.n_samples-1); + float S = S_1 + (pressure - old_avg)*(pressure - calibrationData.avg); + calibrationData.var = S/calibrationData.n_samples; + + if (calibrationData.n_samples >= CALIBRATION_N_SAMPLES) { + sEventBroker->post({EV_ADA_CALIBRATION_COMPLETE}, TOPIC_ADA); + } + break; + } + + case ADAState::IDLE: + { + // Idle state: do nothing + break; + } + + case ADAState::SHADOW_MODE: + { + // Shadow mode state: update kalman, DO NOT send events + Matrix y{1, 1, &pressure}; + filter.update(y); + // Check if the "pressure speed" (hence positive when decending) is + // positive + if (filter.X(1) > 0) + { + ApogeeDetected apogee_det; + apogee_det.tick = miosix::getTick(); + apogee_det.state = status.state; + logger.log(apogee_det); + } + break; + } + + case ADAState::ACTIVE: + { + // Active state send notifications for apogee + Matrix y{1, 1, &pressure}; + filter.update(y); + // Check if the "pressure speed" (hence positive when decending) is + // positive + if (filter.X(1) > 0) + { + sEventBroker->post({EV_ADA_APOGEE_DETECTED}, TOPIC_ADA); + ApogeeDetected apogee_det; + apogee_det.tick = miosix::getTick(); + apogee_det.state = status.state; + logger.log(apogee_det); + } + break; + } + + case ADAState::FIRST_DESCENT_PHASE: + { + // Descent state: send notifications for target altitude reached + Matrix y{1, 1, &pressure}; + filter.update(y); + if (filter.X(0) >= dpl_target_pressure_v) + { + sEventBroker->post({EV_DPL_ALTITUDE}, TOPIC_ADA); + DplPressureReached dpl_reached; + dpl_reached.tick = miosix::getTick(); + logger.log(dpl_reached); + } + break; + } + + case ADAState::END: + { + // End state: do nothing + break; + } + + case ADAState::UNDEFINED: + { + TRACE("ADA Update: Undefined state value \n"); + } + + default: + { + TRACE("ADA Update: Unexpected state value \n"); + } + } +} + +/* --- STATES --- */ +/** + * \brief Calibrating state: the ADA calibrates the initial state. This is the + * initial state. + * + * In this state a call to update() will result in a pressure sample being added + * to the average. + * The exiting transition to the idle state is triggered by a timeout event. + */ +void ADA::stateCalibrating(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + TRACE("ADA: Entering stateCalibrating\n"); + status.state = ADAState::CALIBRATING; + logger.log(status); + break; + case EV_EXIT: + TRACE("ADA: Exiting stateCalibrating\n"); + break; + case EV_ADA_CALIBRATION_COMPLETE: + transition(&ADA::stateIdle); + break; + case EV_TC_SET_DPL_PRESSURE: + const DeploymentPressureEvent& dpl_ev = + static_cast<const DeploymentPressureEvent&>(ev); + dpl_target_pressure_v = dpl_ev.dplPressure; + break; + default: + TRACE("ADA stateCalibrating: %d event not handled", ev.sig); + break; + } +} + +/** + * \brief Idle state: ADA is ready and waiting for liftoff + * + * In this state a call to update() will have no effect. + * The exiting transition to the shadow mode state is triggered by the liftoff + * event. + */ +void ADA::stateIdle(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + TRACE("ADA: Entering stateIdle\n"); + status.state = ADAState::IDLE; + logger.log(status); + filter.X(0) = calibrationData.avg; // Initialize the state with the average + break; + case EV_EXIT: + TRACE("ADA: Exiting stateIdle\n"); + break; + case EV_LIFTOFF: + transition(&ADA::stateShadowMode); + break; + case EV_TC_SET_DPL_PRESSURE: + const DeploymentPressureEvent& dpl_ev = + static_cast<const DeploymentPressureEvent&>(ev); + dpl_target_pressure_v = dpl_ev.dplPressure; + break; + case EV_TC_RESET_CALIBRATION: + transition(&ADA::stateCalibrating); + break; + default: + TRACE("ADA stateIdle: %d event not handled", ev.sig); + break; + } +} + +/** + * \brief Shadow mode state: ADA is running and logging apogees detected, but + * is not generating events + * + * In this state a call to update() will trigger a one step update of the kalman + * filter followed by a check of vertical speed sign. + * The exiting transition to the active state is triggered by a timeout event. + */ +void ADA::stateShadowMode(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + TRACE("ADA: Entering stateShadowMode\n"); + status.state = ADAState::SHADOW_MODE; + logger.log(status); + break; + case EV_EXIT: + TRACE("ADA: Exiting stateShadowMode\n"); + break; + case EV_TIMEOUT_SHADOW_MODE: + transition(&ADA::stateActive); + break; + default: + TRACE("ADA stateShadowMode: %d event not handled", ev.sig); + break; + } +} + +/** + * \brief Active state: ADA is running and it generates an event whe apogee is + * detected + * + * In this state a call to update() will trigger a one step update of the kalman + * filter followed by a check of vertical speed sign. + * The exiting transition to the descent state is triggered by the apogee + * reached event (NOT self generated!) + */ +void ADA::stateActive(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + TRACE("ADA: Entering stateActive\n"); + status.state = ADAState::ACTIVE; + logger.log(status); + break; + case EV_EXIT: + TRACE("ADA: Exiting stateActive\n"); + break; + case EV_APOGEE: + transition(&ADA::stateFirstDescentPhase); + break; + default: + TRACE("ADA stateActive: %d event not handled", ev.sig); + break; + } +} + +/** + * \brief First descent phase state: ADA is running and it generates an event + * when a set pressure (pressure of parachute dpl altitude) is reached + * + * In this state a call to update() will trigger a one step update of the kalman + * filter followed by a check of the pressure. + * The exiting transition to the stop state is triggered by the parachute + * deployment altitude reached event (NOT self generated!) + */ +void ADA::stateFirstDescentPhase(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + TRACE("ADA: Entering stateFirstDescentPhase\n"); + status.state = ADAState::FIRST_DESCENT_PHASE; + logger.log(status); + break; + case EV_EXIT: + TRACE("ADA: Exiting stateFirstDescentPhase\n"); + break; + case EV_DPL_ALTITUDE: + transition(&ADA::stateEnd); + break; + default: + TRACE("ADA stateFirstDescentPhase: %d event not handled", ev.sig); + break; + } +} + +/** + * \brief End state: ADA is stopped + * + * In this state a call to update() will have no effect. + * This is the final state + */ +void ADA::stateEnd(const Event& ev) +{ + switch (ev.sig) + { + case EV_ENTRY: + TRACE("ADA: Entering stateEnd\n"); + status.state = ADAState::END; + logger.log(status); + break; + case EV_EXIT: + TRACE("ADA: Exiting stateEnd\n"); + break; + default: + TRACE("ADA stateEnd: %d event not handled", ev.sig); + break; + } +} + +} // namespace FMM +} // namespace HomeoneBoard diff --git a/src/boards/Homeone/ADA/ADA.h b/src/boards/Homeone/ADA/ADA.h new file mode 100644 index 0000000000000000000000000000000000000000..b18cf06d2182e75461a74d85b425a1f7fd0f7818 --- /dev/null +++ b/src/boards/Homeone/ADA/ADA.h @@ -0,0 +1,60 @@ +/* Copyright (c) 2018 Skyward Experimental Rocketry + * Authors: Luca Mozzarelli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <boards/Homeone/ADA/ADAStatus.h> +#include <events/FSM.h> +#include <kalman/Kalman.h> +#include "logger/LogProxy.h" + +class ADA : public FSM<ADA> +{ + +public: + ADA(); + ~ADA() {} + void update(float pressure); + +private: + void stateCalibrating(const Event& ev); + void stateIdle(const Event& ev); + void stateShadowMode(const Event& ev); + void stateActive(const Event& ev); + void stateFirstDescentPhase(const Event& ev); + void stateEnd(const Event& ev); + + uint16_t cal_delayed_event_id = 0; // Event id for calibration timeout + ADAStatus status; // Variable to store state + + Kalman filter; // Filter object that perfroms the computations + + // Calibration variables + ADACalibrationData calibrationData; + + // Sum of all values squared divided by their number, to comupte variance + float avg_of_squares = 0.0; + + // Parachute deployment pressure volts + uint16_t dpl_target_pressure_v = 5000; // Set default value here + + // Logger + LoggerProxy& logger = *(LoggerProxy::getInstance()); +}; diff --git a/src/boards/Homeone/ADA/ADAStatus.h b/src/boards/Homeone/ADA/ADAStatus.h new file mode 100644 index 0000000000000000000000000000000000000000..e7dd0f43c1d6d5ee9a8b0fa69a866c2b44d9bf60 --- /dev/null +++ b/src/boards/Homeone/ADA/ADAStatus.h @@ -0,0 +1,56 @@ +/* Copyright (c) 2018 Skyward Experimental Rocketry + * Authors: Luca Mozzarelli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +// All possible states of the ADA FMM +enum class ADAState { + UNDEFINED, + CALIBRATING, + IDLE, + SHADOW_MODE, + ACTIVE, + FIRST_DESCENT_PHASE, + END +}; + +// Struct to log current state +struct ADAStatus +{ + ADAState state = ADAState::UNDEFINED; +}; + +// Struct of calibration data +struct ADACalibrationData { + float var = 0.0; // Sample variance + int n_samples = 0; // Number of samples collected + float avg = 0.0; // Average pressure +}; + +// Struct to log apogee detection +struct ApogeeDetected { + ADAState state; + long long tick; +}; + +// Struct to log deployment pressure detection +struct DplPressureReached { + long long tick; +} \ No newline at end of file diff --git a/src/boards/Homeone/ADA/ADA_config.h b/src/boards/Homeone/ADA/ADA_config.h new file mode 100644 index 0000000000000000000000000000000000000000..d87c6ff46385f375d2c320235fded12a65d1cae4 --- /dev/null +++ b/src/boards/Homeone/ADA/ADA_config.h @@ -0,0 +1,41 @@ +/* Copyright (c) 2018 Skyward Experimental Rocketry + * Authors: Luca Mozzarelli + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#warning "ADA COSTANTS ARE ONLY PLACEHOLDER VALUES" + +namespace HomeoneBoard +{ +namespace FMM +{ +// TODO: Change with real values + +// State timeouts +static const unsigned int TIMEOUT_MS_CALIBRATION = 15 * 1000; +static const unsigned int CALIBRATION_N_SAMPLES = 5000; + +// Kalman parameters +float P_data[9] = {0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1}; // Initial error covariance matrix +float R_data[1] = {10}; // Measurement variance +float Q_data[9] = {0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01}; // Model variance matrix +float samplingPeriod = 0.01; // In seconds +} +} \ No newline at end of file diff --git a/src/boards/Homeone/EventClasses.h b/src/boards/Homeone/EventClasses.h index 2263f24eac78484b498289badaa6d7c2cc664327..9173f91fd1a75d9c2cf4d4ab0605d8e8595f0c65 100644 --- a/src/boards/Homeone/EventClasses.h +++ b/src/boards/Homeone/EventClasses.h @@ -30,10 +30,9 @@ namespace HomeoneBoard { -struct AltimeterCalibrationEvent : Event +struct DeploymentPressureEvent : Event { - uint16_t T0; // Calibration temperature - uint16_t P0; // Calibration pressure + uint16_t dplPressure; // Deployment pressure }; struct PressureSampleEvent : Event diff --git a/src/boards/Homeone/EventFunctions.cpp b/src/boards/Homeone/EventFunctions.cpp index c7df8455dbabb39608423f2210910990dcfbcb51..2dd856bc16f2e8fe8e5c62238486cb50e2395b12 100644 --- a/src/boards/Homeone/EventFunctions.cpp +++ b/src/boards/Homeone/EventFunctions.cpp @@ -27,7 +27,8 @@ */ // Generated from: https://docs.google.com/spreadsheets/d/12TecOmDd7Uot-MvXkCbhDJRU48-XO6s5ChKDlr4AOvI -// Autogen date: 2018-12-13 18:31:00.418616 +// Autogen date: 2018-12-13 19:38:35.317781 + #include "Events.h" @@ -42,6 +43,7 @@ string getEventString(uint8_t event) { static const map<uint8_t, string> event_string_map { { EV_ADA_APOGEE_DETECTED, "EV_ADA_APOGEE_DETECTED" }, + { EV_ADA_CALIBRATION_COMPLETE, "EV_ADA_CALIBRATION_COMPLETE" }, { EV_ADA_DPL_ALT_DETECTED, "EV_ADA_DPL_ALT_DETECTED" }, { EV_APOGEE, "EV_APOGEE" }, { EV_ARMED, "EV_ARMED" }, @@ -78,6 +80,7 @@ string getEventString(uint8_t event) { EV_TC_NC_OPEN, "EV_TC_NC_OPEN" }, { EV_TC_START_LOGGING, "EV_TC_START_LOGGING" }, { EV_TC_STOP_LOGGING, "EV_TC_STOP_LOGGING" }, + { EV_TC_RESET_CALIBRATION, "EV_TC_RESET_CALIBRATION" }, { EV_TC_TEST_MODE, "EV_TC_TEST_MODE" }, { EV_TIMEOUT_APOGEE, "EV_TIMEOUT_APOGEE" }, { EV_TIMEOUT_ARM, "EV_TIMEOUT_ARM" }, diff --git a/src/boards/Homeone/Events.h b/src/boards/Homeone/Events.h index 334c1ed4f30115dcafb306dc0702a6c5ee77ae5e..e2beb11f716fadf900c1c3bfb7b0179a2bcb22f1 100644 --- a/src/boards/Homeone/Events.h +++ b/src/boards/Homeone/Events.h @@ -27,7 +27,8 @@ */ // Generated from: https://docs.google.com/spreadsheets/d/12TecOmDd7Uot-MvXkCbhDJRU48-XO6s5ChKDlr4AOvI -// Autogen date: 2018-12-13 18:31:00.418616 +// Autogen date: 2018-12-13 19:38:35.317781 + #ifndef SRC_SHARED_BOARDS_HOMEONE_EVENTS_H #define SRC_SHARED_BOARDS_HOMEONE_EVENTS_H @@ -52,6 +53,7 @@ namespace HomeoneBoard enum Events : uint8_t { EV_ADA_APOGEE_DETECTED = EV_FIRST_SIGNAL, + EV_ADA_CALIBRATION_COMPLETE, EV_ADA_DPL_ALT_DETECTED, EV_APOGEE, EV_ARMED, @@ -88,6 +90,7 @@ enum Events : uint8_t EV_TC_NC_OPEN, EV_TC_START_LOGGING, EV_TC_STOP_LOGGING, + EV_TC_RESET_CALIBRATION, EV_TC_TEST_MODE, EV_TIMEOUT_APOGEE, EV_TIMEOUT_ARM, diff --git a/src/boards/Homeone/SensorManager/SensorManager.h b/src/boards/Homeone/SensorManager/SensorManager.h index 5e39d2d31dc94dd4ec13c97f683321d99b54b6d3..841dc5d67d0185d58adef1ad51a801ee29f8c818 100644 --- a/src/boards/Homeone/SensorManager/SensorManager.h +++ b/src/boards/Homeone/SensorManager/SensorManager.h @@ -40,7 +40,7 @@ using std::vector; // Forward declarations class TestSensor; -template <typename BusI2C> +template <typename BusI2C, typename BusyPin, typename CONVST> class AD7994; template <typename BusSPI> @@ -56,7 +56,7 @@ class ADIS16405; namespace HomeoneBoard { // Type definitions -typedef AD7994<busI2C1> AD7994Type; +typedef AD7994<busI2C1, ad7994_busy_pin, ad7994_nconvst> AD7994Type; typedef MPU9250<spiMPU9250> MPU9250Type; typedef MAX21105<spiMAX21105> MAX21105Type; // typedef ADIS16405<spiADIS16405> ADIS16405Type; diff --git a/src/boards/Homeone/TMTCManager/TCHandler.h b/src/boards/Homeone/TMTCManager/TCHandler.h index 8e505c785b84b5f4ce32b3ca447e2e7df44150c7..42b808f3a82b03daaf188677807e3ed359dcd521 100644 --- a/src/boards/Homeone/TMTCManager/TCHandler.h +++ b/src/boards/Homeone/TMTCManager/TCHandler.h @@ -81,7 +81,7 @@ static void handleMavlinkMessage(MavSender* sender, const mavlink_message_t& msg { sendAck(sender, msg); - /* Reschedule GS_OFFLINE evetn */ + /* Reschedule GS_OFFLINE event */ sEventBroker->removeDelayed(g_gsOfflineEvId); g_gsOfflineEvId = sEventBroker->postDelayed(Event{EV_GS_OFFLINE}, GS_OFFLINE_TIMEOUT); diff --git a/src/boards/Homeone/Topics.h b/src/boards/Homeone/Topics.h index 9e1b72dc0f68ecab7e91c8f8d0e5a9737ab55ddb..d0ad0e713e25bfebff0fcf8876fe5082c8c356ac 100644 --- a/src/boards/Homeone/Topics.h +++ b/src/boards/Homeone/Topics.h @@ -27,7 +27,8 @@ */ // Generated from: https://docs.google.com/spreadsheets/d/12TecOmDd7Uot-MvXkCbhDJRU48-XO6s5ChKDlr4AOvI -// Autogen date: 2018-12-13 18:31:00.418616 +// Autogen date: 2018-12-13 19:38:35.317781 + #ifndef SRC_SHARED_BOARDS_HOMEONE_TOPICS_H #define SRC_SHARED_BOARDS_HOMEONE_TOPICS_H diff --git a/src/boards/Homeone/configs/SensorManagerConfig.h b/src/boards/Homeone/configs/SensorManagerConfig.h index 2d04f91e3992f8b4779882a987a53d298423464b..f1930b11adb24b781f890fe9cc7f9e446f5ce8c1 100644 --- a/src/boards/Homeone/configs/SensorManagerConfig.h +++ b/src/boards/Homeone/configs/SensorManagerConfig.h @@ -45,6 +45,9 @@ typedef ProtocolSPI<busSPI1, miosix::sensors::mpu9250::cs> spiMPU9250; typedef ProtocolSPI<busSPI1, miosix::sensors::max21105::cs> spiMAX21105; typedef ProtocolSPI<busSPI1, miosix::sensors::adis16405::cs> spiADIS16405; +typedef miosix::sensors::ad7994::ab ad7994_busy_pin; +typedef miosix::sensors::ad7994::nconvst ad7994_nconvst; + static const uint8_t AD7994_I2C_ADDRESS = 0x24; // Todo: Update with real value } // namespace HomeoneBoard diff --git a/src/boards/Ignition/IgnitionManager.cpp b/src/boards/Ignition/IgnitionManager.cpp deleted file mode 100644 index 107d80f8ffbf66f0c149a7801cf7459a7c30c93c..0000000000000000000000000000000000000000 --- a/src/boards/Ignition/IgnitionManager.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Authors: Alvise de'Faveri Tron - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ -#pragma once - -#include "IgnitionManager.h" - -namespace IgnBoard -{ - -using namespace actuators; - -/** - * Canbus receiving function. - */ -void canRcv(CanMsg message, IgnitionManager* mngr) -{ - TRACE("[CAN] Received message with id %d\n", message.StdId); - - switch (message.StdId) - { - case CanInterfaces::CAN_TOPIC_HOMEONE: - if(message.Data[0] == CanInterfaces::CAN_MSG_ABORT) { - myStatus.u1_abort_cmd = 0b1; - mngr->abort(); - } - else if(message.Data[0] == CanInterfaces::CAN_MSG_REQ_IGN_STATUS) { - mngr->getStatus(); - } - else { - TRACE("[CAN] Message not recognized\n"); - } - break; - - case CanInterfaces::CAN_TOPIC_LAUNCH: - uint64_t code = 0; - memcpy(&code, message.Data, 8); - - mngr->launch(code); - break; - - default: - TRACE("[CAN] Message not recognized\n"); - break; - } -} - - -/* Manager constructor: init canbus, set internal state and send state on canbus */ -IgnitionManager::IgnitionManager() -{ - CanManager c = new CanManager(CAN1); - initCanbus(c); - - // Communication with board 2? - - myStatus = 0; - sendStatus(); -} - -/* Initialise CAN1 on PA11, PA12, set filters and set receiver function. */ -void IgnitionManager::initCanbus(CanManager& c) -{ - /* Initialise canbus with hardware filters. */ - c.addHWFilter(CanInterfaces::CAN_TOPIC_HOMEONE, 0); - c.addHWFilter(CanInterfaces::CAN_TOPIC_LAUNCH, 0); - - canbus_init_t can_conf = { - CAN1, miosix::Mode::ALTERNATE, 9, {CAN1_RX0_IRQn, CAN1_RX1_IRQn}}; - - /* Transform canRcv(message, mngr) into canRcv(message) (see std::bind reference). */ - CanHandler handler = std::bind(canRcv, std::placeholders::_1, this); - - /* Add canbus and define pins, configuration and receiving function. */ - c.addBus<GPIOA_BASE, 11, 12>(can_conf, &handler); - - TRACE("[CAN] Initialised CAN1 on PA11-12 \n"); -} - - -/* Ignition Functions */ - -void IgnitionManager::abort() -{ - abortPin::low(); - Thread::sleep(ABORT_DURATION); - abortPin::high(); - - sendStatus(); -} - -void IgnitionManager::getStatus() -{ - // getStatus from other board - // refresh myStatus - sendStatus(); -} - -void IgnitionManager::launch(uint64_t launch_code) -{ - if(myStatus.u1_abort_cmd == 0) - { - if(checkLaunchCode(launch_code)) - { - // send launch code to other board - // poll for response - // if response negativa - // myStatus.u2_wrong_code = 0b1; - // abort() - // else if nCycle > 1000 - // myStatus.u1_abort_timeout = 0b1; - // abort() - // else - ignitionPin::high(); - Thread::sleep(LAUNCH_DURATION); - ignitionPin::low(); - } - else { - myStatus.u1_wrong_code = 0b1; - abort(); - } - } - else { - TRACE("Received launch while aborted\n"); - } -} - -void IgnitionManager::sendStatus() -{ - canManager->getBus(0)->send(CanInterfaces::CAN_MSG_IGN_STATUS, - (uint8_t*)myStatus, sizeof(IgnitionBoardStatus)); -} - -bool IgnitionManager::checkLaunchCode(uint64_t launch_code) -{ - return launch_code == EXPECTED_LAUNCH_CODE; -} - -} diff --git a/src/boards/Ignition/IgnitionManager.h b/src/boards/Ignition/IgnitionManager.h deleted file mode 100644 index 01ae9252602ce6da89920d523a9be322b5aa6712..0000000000000000000000000000000000000000 --- a/src/boards/Ignition/IgnitionManager.h +++ /dev/null @@ -1,81 +0,0 @@ -/* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Authors: Alvise de'Faveri Tron - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ -#pragma once - -#include <Common.h> -#include <drivers/canbus/CanManager.h> -#include <drivers/canbus/CanUtils.h> -#include <interfaces-impl/hwmapping.h> - -#include <boards/CanInterfaces.h> - -// TODO SPI - -namespace IgnBoard -{ - - -/** - * Implementation of the Ignition Board logic. - */ -class IgnitionManager -{ - -public: - IgnitionManager(); - ~IgnitionManager() {} - - void abort(); - void getStatus(); - void launch(uint64_t launch_code); - - void sendStatus(); - bool checkLaunchCode(uint64_t launch_code); - -private: - bool isAborted = false; - CanInterfaces::IgnitionBoardStatus myStatus; - - CanManager c; - - static const uint64_t EXPECTED_LAUNCH_CODE = 0xAABB; - static const uint32_t ABORT_DURATION = 10000; - static const uint32_t LAUNCH_DURATION = 10000; - - /** - * @brief Initialise CAN, set hardware filters and receiver function. - * - * @param c CanManager to which the bus has to be added. - */ - void initCanbus(CanManager& c); - -}; - -/** - * @brief Canbus receiving function. - * - * @param message each message received on the CANBUS (with the HW filters) - * @param mngr IgnitionManager that implements the ignition functions - */ -void canRcv(CanMsg message, IgnitionManager* mngr); - -}