From c0cd46e37d2056c82c69a646444689b9fa8fb33e Mon Sep 17 00:00:00 2001
From: Luca Erbetta <luca.erbetta@skywarder.eu>
Date: Wed, 6 Nov 2019 23:45:36 +0100
Subject: [PATCH] Updated state machines
---
.../event_header_generator/Events.h.template | 1 -
.../DeploymentController.cpp | 18 ++++++++++++++++
.../DeploymentController.h | 6 ++++++
.../FlightModeManager/FlightModeManager.cpp | 6 ++++++
.../SensorManager/SensorManager.cpp | 21 +++++++++----------
.../DeathStack/TMTCManager/TMTCManager.cpp | 4 ++--
.../DeathStack/TMTCManager/TMTCManager.h | 1 -
.../DeathStack/configs/DeploymentConfig.h | 5 +++--
8 files changed, 45 insertions(+), 17 deletions(-)
diff --git a/scripts/event_header_generator/Events.h.template b/scripts/event_header_generator/Events.h.template
index 456d25118..3d79b5859 100644
--- a/scripts/event_header_generator/Events.h.template
+++ b/scripts/event_header_generator/Events.h.template
@@ -37,7 +37,6 @@
#include "events/Event.h"
#include "events/EventBroker.h"
-#include "EventStrings.h"
#include "Topics.h"
using std::string;
diff --git a/src/boards/DeathStack/DeploymentController/DeploymentController.cpp b/src/boards/DeathStack/DeploymentController/DeploymentController.cpp
index b1dc60205..6df81f36d 100644
--- a/src/boards/DeathStack/DeploymentController/DeploymentController.cpp
+++ b/src/boards/DeathStack/DeploymentController/DeploymentController.cpp
@@ -132,6 +132,11 @@ State DeploymentController::state_idle(const Event& ev)
resetServo();
break;
}
+ case EV_WIGGLE_SERVO
+ {
+ wiggleServo();
+ break;
+ }
default:
{
retState = tran_super(&DeploymentController::Hsm_top);
@@ -407,4 +412,17 @@ void DeploymentController::disableServo()
ejection_servo.disable(SERVO_CHANNEL);
}
+void DeploymentController::wiggleServo()
+{
+ for (int i = 0; i < 3; i++)
+ {
+ ejection_servo.setPosition(SERVO_CHANNEL, SERVO_RESET_POS + SERVO_WIGGLE_AMPLITUDE);
+ Thread::sleep(500);
+ ejection_servo.setPosition(SERVO_CHANNEL, SERVO_RESET_POS - SERVO_WIGGLE_AMPLITUDE);
+ Thread::sleep(500);
+ ejection_servo.setPosition(SERVO_CHANNEL, SERVO_RESET_POS);
+ Thread::sleep(500);
+ }
+}
+
} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/DeploymentController/DeploymentController.h b/src/boards/DeathStack/DeploymentController/DeploymentController.h
index fca36fe51..c8d0f9db3 100644
--- a/src/boards/DeathStack/DeploymentController/DeploymentController.h
+++ b/src/boards/DeathStack/DeploymentController/DeploymentController.h
@@ -90,6 +90,12 @@ private:
void ejectNosecone();
void disableServo();
+ /**
+ * @brief Wiggle the servo just a bit around the reset position to show it's
+ * working
+ */
+ void wiggleServo();
+
/**
* Defer an event to be processed when the state machine goes back to
* state_idle
diff --git a/src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp b/src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp
index 304466402..2e50d0c57 100644
--- a/src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp
@@ -339,6 +339,7 @@ State FlightModeManager::state_armed(const Event& ev)
break;
}
case EV_UMBILICAL_DETACHED:
+ case EV_TC_LAUNCH:
{
retState = transition(&FlightModeManager::state_ascending);
break;
@@ -412,6 +413,11 @@ State FlightModeManager::state_testing(const Event& ev)
sEventBroker->post(Event{EV_RESET_SERVO}, TOPIC_DEPLOYMENT);
break;
}
+ case EV_TC_WIGGLE_SERVO:
+ {
+ sEventBroker->post(Event{EV_WIGGLE_SERVO}, TOPIC_DEPLOYMENT);
+ break;
+ }
case EV_TC_CLOSE_LOG:
{
logger.stop();
diff --git a/src/boards/DeathStack/SensorManager/SensorManager.cpp b/src/boards/DeathStack/SensorManager/SensorManager.cpp
index 1ab94e8b1..de2107d07 100644
--- a/src/boards/DeathStack/SensorManager/SensorManager.cpp
+++ b/src/boards/DeathStack/SensorManager/SensorManager.cpp
@@ -122,16 +122,24 @@ void SensorManager::initSensors()
sensor_status.piksi = 1;
// Initialization
+ TRACE("Mpu init\n");
sensor_status.mpu9250 = imu_mpu9250->init();
// sensor_status.adis = imu_adis16405->init();
+ TRACE("LM75b IMU init\n");
sensor_status.lm75b_imu = temp_lm75b_imu->init();
+
+ TRACE("LM75b ANAL init\n");
+
sensor_status.lm75b_analog = temp_lm75b_analog->init();
// // TODO: lsm6ds3h
- sensor_status.ms5803 = pressure_ms5803->init();
+ TRACE("MS5803 init\n");
+ sensor_status.ms5803 = pressure_ms5803->init();
+ TRACE("AD7994 init\n");
+
sensor_status.ad7994 = adc_ad7994->init();
sensor_status.battery_sensor = adc_internal->getBatterySensorPtr()->init();
@@ -254,16 +262,7 @@ void SensorManager::stateIdle(const Event& ev)
// Perform the transition in both cases
case EV_TC_START_SENSOR_LOGGING:
- case EV_LIFTOFF:
-
-#ifdef USE_MOCK_SENSORS
- // Signal to the mock pressure sensor that we have liftoff in order
- // to start simulating flight pressures
- if (ev.sig == EV_LIFTOFF)
- {
- mock_pressure_sensor->before_liftoff = false;
- }
-#endif
+ case EV_ARMED:
transition(&SensorManager::stateLogging);
break;
diff --git a/src/boards/DeathStack/TMTCManager/TMTCManager.cpp b/src/boards/DeathStack/TMTCManager/TMTCManager.cpp
index c350f125f..6546c580c 100644
--- a/src/boards/DeathStack/TMTCManager/TMTCManager.cpp
+++ b/src/boards/DeathStack/TMTCManager/TMTCManager.cpp
@@ -75,7 +75,7 @@ void TMTCManager::stateGroundTM(const Event& ev)
test_tm_event_id = sEventBroker->postDelayed<TEST_TM_TIMEOUT>(
Event{EV_SEND_TEST_TM}, TOPIC_TMTC);
- TRACE("[TMTC] Entering stateTestTM\n");
+ TRACE("[TMTC] Entering stateGroundTM\n");
StackLogger::getInstance()->updateStack(THID_TMTC_FSM);
break;
@@ -118,7 +118,7 @@ void TMTCManager::stateGroundTM(const Event& ev)
{
sEventBroker->removeDelayed(test_tm_event_id);
- TRACE("[TMTC] Exiting stateTestTM\n");
+ TRACE("[TMTC] Exiting stateGroundTM\n");
break;
}
default:
diff --git a/src/boards/DeathStack/TMTCManager/TMTCManager.h b/src/boards/DeathStack/TMTCManager/TMTCManager.h
index 2aa2b8710..da3e07f9d 100644
--- a/src/boards/DeathStack/TMTCManager/TMTCManager.h
+++ b/src/boards/DeathStack/TMTCManager/TMTCManager.h
@@ -74,7 +74,6 @@ private:
// void stateIdle(const Event& ev);
void stateGroundTM(const Event& ev);
void stateFlightTM(const Event& ev);
- void stateTestTM(const Event& ev);
inline void packHRTelemetry(uint8_t* packet, unsigned int index);
inline void packLRTelemetry(uint8_t* packet);
diff --git a/src/boards/DeathStack/configs/DeploymentConfig.h b/src/boards/DeathStack/configs/DeploymentConfig.h
index 99b32893b..3c71bc688 100644
--- a/src/boards/DeathStack/configs/DeploymentConfig.h
+++ b/src/boards/DeathStack/configs/DeploymentConfig.h
@@ -38,8 +38,7 @@ namespace DeploymentConfigs
static constexpr uint8_t MAX_EJECTION_ATTEMPTS = 1;
-static constexpr int NC_OPEN_TIMEOUT = 10000;
-static constexpr int SERVO_RESET_TIMEOUT = 1000;
+static constexpr int NC_OPEN_TIMEOUT = 5000;
static const PWM::Timer SERVO_TIMER{
TIM4, &(RCC->APB1ENR), RCC_APB1ENR_TIM4EN,
@@ -49,6 +48,8 @@ static constexpr PWMChannel SERVO_CHANNEL = PWMChannel::CH1;
// Servo rest position
static constexpr float SERVO_RESET_POS = 0.77f;
+static constexpr float SERVO_WIGGLE_AMPLITUDE = 0.02f;
+
// Servo position when ejecting the nosecone
static constexpr float SERVO_EJECT_POS = 0.45f;
--
GitLab