diff --git a/.gitignore b/.gitignore
index d2d63ed98612b69e0bb748cd74a300734f41e1a2..9df3b454f410d9279fb0a25c11ee4fbb2d9e752c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -23,7 +23,9 @@ build
*.sublime-project
.vscode/*
-
store.json
scripts/event_header_generator/generated/
scripts/event_header_generator/venv/
+
+**/generated
+**/scxmls
diff --git a/sbs.conf b/sbs.conf
index 03c0ccdf69feabf26b3ac20bce1bbf7860df84b5..d82060509c6cd32a126c1e1598ea69d3be627e0a 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -49,6 +49,7 @@
#stm32f100c8_microboard
#stm32f469ni_stm32f469i-disco
#stm32f429zi_skyward_death_stack
+#stm32f429zi_skyward_death_stack_x
#stm32f401re_nucleo
#stm32f103c8_skyward_aldeeran
@@ -76,10 +77,13 @@ ENTRY_PATH: src/entrypoints
TESTS_PATH: src/tests
SRC_PATH: src
SBS_BASE: skyward-boardcore
-PROJECT_INCLUDES: -Isrc/boards/DeathStack
+PROJECT_INCLUDES: -Isrc/boards
+ -Isrc/boards/DeathStack
-Iskyward-boardcore/src/shared
-Iskyward-boardcore
+ -Iskyward-boardcore/src/shared
-Iskyward-boardcore/libs/mavlink_skyward_lib
+ -Iskyward-boardcore/libs/eigen
PROJECT_SUBDIRS:
PROJECT_LIBS:
@@ -97,11 +101,9 @@ Files: src/boards/DeathStack/LoggerService/LoggerService.cpp
src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp
src/boards/DeathStack/SensorManager/SensorManager.cpp
src/boards/DeathStack/DeploymentController/DeploymentController.cpp
- src/boards/DeathStack/DeploymentController/Motor/MotorDriver.cpp
src/boards/DeathStack/PinHandler/PinHandler.cpp
src/boards/DeathStack/TMTCManager/TMTCManager.cpp
src/boards/DeathStack/LoggerService/TmRepository.cpp
- src/boards/DeathStack/ADA/ADAController.cpp
src/boards/DeathStack/ADA/ADA.cpp
src/boards/DeathStack/ADA/ADACalibrator.cpp
src/boards/DeathStack/ADA/DeploymentUtils/generated/elevation_map_data.cpp
@@ -111,16 +113,18 @@ Files: src/boards/DeathStack/LoggerService/LoggerService.cpp
[ada]
Type: srcfiles
-Files: src/boards/DeathStack/ADA/ADAController.cpp
- src/boards/DeathStack/ADA/ADA.cpp
+Files: src/boards/DeathStack/ADA/ADA.cpp
src/boards/DeathStack/ADA/ADACalibrator.cpp
- src/boards/DeathStack/ADA/DeploymentUtils/generated/elevation_map_data.cpp
src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.cpp
+ src/boards/DeathStack/ADA/DeploymentUtils/generated/elevation_map_data.cpp
[deployment]
Type: srcfiles
-Files: src/boards/DeathStack/DeploymentController/Motor/MotorDriver.cpp
- src/boards/DeathStack/DeploymentController/DeploymentController.cpp
+Files: src/boards/DeathStack/DeploymentController/DeploymentController.cpp
+
+[pinhandler]
+Type: srcfiles
+Files: src/boards/DeathStack/PinHandler/PinHandler.cpp
[fmm]
Type: srcfiles
@@ -173,6 +177,7 @@ Files: src/tests/ram_test/sha1.cpp
Type: srcfiles
Files: src/boards/DeathStack/events/EventStrings.cpp
+
#--------------------------#
# Boards #
#--------------------------#
@@ -185,19 +190,19 @@ Files: src/boards/DeathStack/events/EventStrings.cpp
[death-stack-entry]
Type: board
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: death-stack-entry
-Include: %shared %deathstack %logger %pwm %xbee %piksi %servo %mock-sensors-data
-Defines: -DDEATH_STACK_1 -DUSE_MOCK_SENSORS
+Include: %shared %deathstack %logger %pwm %hbridge %spi %xbee %piksi %servo
+Defines: -DDEATH_STACK_1
Main: death-stack-entry
-[death-stack-testsuite]
-Type: board
-BoardId: stm32f429zi_skyward_death_stack
-BinName: death-stack-testsuite
-Include: %shared %deathstack %logger %pwm %xbee %piksi %servo
-Defines: -DDEATH_STACK_1
-Main: death-stack-testsuite
+# [death-stack-testsuite]
+# Type: board
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: death-stack-testsuite
+# Include: %shared %deathstack %logger %pwm %xbee %piksi %servo
+# Defines: -DDEATH_STACK_1
+# Main: death-stack-testsuite
#--------------------------#
# Tests #
@@ -211,41 +216,41 @@ Main: death-stack-testsuite
[catch-tests-entry]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: catch-tests-entry
-Include: %shared %deathstack %logger %pwm %xbee %piksi %servo %hermes-tests
+Include: %shared %deathstack %logger %pwm %xbee %piksi %servo %hermes-tests %ada-test-sources %mock-sensors-data
Defines: -DDEATH_STACK_1
Main: catch/catch-tests-entry
## Driver tests
-[test-imus]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack
-BinName: test-imus
-Include: %shared %sensors %logger %logger-hermes
-Defines: -DDEATH_STACK_1
-Main: drivers/test-imus
+# [test-imus]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-imus
+# Include: %shared %sensors %logger %logger-hermes
+# Defines: -DDEATH_STACK_1
+# Main: drivers/test-imus
[test-cutter]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: test-cutter
-Include: %pwm
-Defines: -DDEBUG -DDEATH_STACK_1
+Include: %pwm %hbridge %internal-adc %shared
+Defines: -DDEBUG
Main: drivers/test-cutter
[test-mavlink]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: test-mavlink
-Include: %shared %tmtc %logger %logger-hermes %xbee %logservice
+Include: %shared %spi %tmtc %logger %logger-hermes %xbee %logservice
Defines: -DDEBUG -DTRACE_EVENTS -DDEATH_STACK_1
Main: drivers/test-mavlink
[ledwave]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: ledwave
Include: %shared
Defines: -DDEBUG
@@ -253,47 +258,55 @@ Main: ledwave
[ramtest]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: ramtest
Include: %ram-test
-Defines:
-Main: ram_test/main
+Defines: -D__ENABLE_XRAM
+Main: ram_test/ramtest
-[test-motor]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack
-BinName: test-motor
-Include: %shared %deployment %pwm %servo
-Defines:
-Main: drivers/test-motor
+# [test-motor]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-motor
+# Include: %shared %deployment %pwm %servo
+# Defines:
+# Main: drivers/test-motor
[test-servo]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: test-servo
Include: %shared %servo %pwm
Defines:
Main: drivers/test-servo
-[test-all-sensors]
+[test-hse]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
-BinName: test-all-sensors
-Include: %shared %logger %piksi
-Defines: -DDEATH_STACK_1
-Main: drivers/test-all-sensors
+BoardId: stm32f429zi_skyward_death_stack_x
+BinName: test-hse
+Include:
+Defines:
+Main: test-hse
-[test-pressure-calib]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack
-BinName: test-pressure-calib
-Include: %shared %logger %piksi
-Defines: -DDEATH_STACK_1
-Main: drivers/test-pressure-calib
+# [test-all-sensors]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-all-sensors
+# Include: %shared %logger %piksi
+# Defines: -DDEATH_STACK_1
+# Main: drivers/test-all-sensors
+
+# [test-pressure-calib]
+# Type: test
+# BoardId: stm32f429zi_skyward_death_stack_x
+# BinName: test-pressure-calib
+# Include: %shared %logger %piksi
+# Defines: -DDEATH_STACK_1
+# Main: drivers/test-pressure-calib
[test-power-board]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: test-power-board
Include: %shared
Defines: -DDEBUG
@@ -304,7 +317,7 @@ Main: drivers/test-power-board
# Cant use this as it is since the DeathStack singleton is required for handling messages in the TCHandler class
# [test-tmtc]
# Type: test
-# BoardId: stm32f429zi_skyward_death_stack
+# BoardId: stm32f429zi_skyward_death_stack_x
# BinName: test-tmtc
# Include: %shared %tmtc %logger %logger-hermes %xbee %evt-functions %logproxy
# Defines: -DDEBUG -DTRACE_EVENTS
@@ -312,15 +325,23 @@ Main: drivers/test-power-board
# [test-canproxy]
# Type: test
-# BoardId: stm32f429zi_skyward_death_stack
+# BoardId: stm32f429zi_skyward_death_stack_x
# BinName: test-canproxy
# Include: %shared %canproxy %logger %logger-hermes %canbus
# Defines: -DDEBUG
# Main: test-canproxy
+[test-pinhandler]
+Type: test
+BoardId: stm32f429zi_skyward_death_stack_x
+BinName: test-pinhandler
+Include: %shared %pinhandler %logger %logger-hermes %logservice
+Defines: -DDEBUG
+Main: test-pinhandler
+
[test-logproxy]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: test-logproxy
Include: %shared %logger %logger-hermes %logservice
Defines: -DDEBUG -DDEATH_STACK_1
@@ -328,32 +349,24 @@ Main: test-logproxy
[test-sensormanager]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: test-sensormanager
-Include: %shared %sensors %logger %logger-hermes %ada %piksi %logservice %ada-test-sources
+Include: %shared %sensors %logger %logger-hermes %piksi %logservice %ada-test-sources
Defines: -DDEBUG -USE_MOCK_SENSORS -DDEATH_STACK_1
Main: test-sensormanager
#Cant use this as it is since the DeathStack singleton is required for handling messages in the TCHandler class
# [test-sm+tmtc]
# Type: test
-# BoardId: stm32f429zi_skyward_death_stack
+# BoardId: st %adam32f429zi_skyward_death_stack
# BinName: test-sm+tmtc
# Include: %shared %sensors %logger %logservice %logger-hermes %ada %piksi %tmtc %xbee %evt-functions %ada-test-sources
# Defines: -DDEBUG -DDEATH_STACK_1
# Main: test-sm+tmtc
-[test-dpl]
-Type: test
-BoardId: stm32f429zi_skyward_death_stack
-BinName: test-dpl
-Include: %shared %deathstack %logger %pwm %xbee %piksi %servo %test-utils
-Defines: -DDEBUG -DTRACE_EVENTS -DDEATH_STACK_1
-Main: test-dpl
-
[test-fmm]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: test-fmm
Include: %deployment %shared %logger %logger-hermes %logservice %evt-functions %fmm %pwm %servo
Defines: -DDEBUG -DDEATH_STACK_1
@@ -361,7 +374,7 @@ Main: test-fmm
#[test-homeone]
#Type: test
-#BoardId: stm32f429zi_skyward_death_stack
+#BoardId: stm32f429zi_stm32f4discovery
#BinName: test-homeone
#Include: %shared
#Defines: -DDEBUG
@@ -372,31 +385,31 @@ Main: test-fmm
[test-ada-simulation]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
+BoardId: stm32f429zi_skyward_death_stack_x
BinName: test-ada-simulation
-Include: %shared %deathstack %logger %pwm %xbee %piksi %servo %ada-test-sources
-Defines: -DSTANDALONE_CATCH1_TEST -DSDRAM_ISSI -DDEATH_STACK_2
+Include: %shared %ada %ada-test-sources %logger %logservice %logger-hermes %spi
+Defines: -DDEBUG -DSTANDALONE_CATCH1_TEST
Main: catch/ada/ada_kalman_p/test-ada-simulation
-[test-kalman-acc]
+[test-ada]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
-BinName: test-kalman-acc
-Include: %shared %deathstack %logger %pwm %xbee %piksi %servo %kalman-test-sources
-Defines: -DSTANDALONE_CATCH1_TEST -DSDRAM_ISSI -DDEATH_STACK_2 -DDEBUG
-Main: catch/ada/kalman_acc/test-kalman-acc
+BoardId: stm32f429zi_skyward_death_stack_x
+BinName: test-ada
+Include: %shared %ada %test-utils %ada-test-sources %mock-sensors-data %logger %logservice %logger-hermes
+Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG
+Main: catch/fsm/test-ada
-# [test-deployment]
+# [test-kalman-acc]
# Type: test
-# BoardId: stm32f429zi_skyward_death_stack
-# BinName: test-deployment
-# Include: %deployment %shared %pwm %logger %logger-hermes %test-utils
-# Defines: -DSTANDALONE_CATCH1_TEST
-# Main: catch/fsm/test-deployment
-#
+# BoardId: stm32f429zi_stm32f4discovery
+# BinName: test-kalman-acc
+# Include: %shared %logger %pwm %xbee %piksi %servo %kalman-test-sources
+# Defines: -DSTANDALONE_CATCH1_TEST -DSDRAM_ISSI -DDEATH_STACK_2 -DDEBUG
+# Main: catch/ada/kalman_acc/test-kalman-acc
+
# [test-ignition]
-# Type: test
-# BoardId: stm32f429zi_skyward_death_stack
+# Type: testada_controller
+# BoardId: stm32f429zi_skyward_death_stack_x
# BinName: test-ignition
# Include: %shared %ignition %logger %logger-hermes %canbus %canproxy %test-utils
# Defines: -DSTANDALONE_CATCH1_TEST
@@ -404,16 +417,48 @@ Main: catch/ada/kalman_acc/test-kalman-acc
#
# [test-fmm]
# Type: test
-# BoardId: stm32f429zi_skyward_death_stack
+# BoardId: stm32f429zi_skyward_death_stack_x
# BinName: test-fmm
# Include: %shared %logger %logger-hermes %test-utils %fmm
# Defines: -DSTANDALONE_CATCH1_TEST
# Main: catch/fsm/test-fmm
-#
-[test-ada]
+
+[test-deployment]
Type: test
-BoardId: stm32f429zi_skyward_death_stack
-BinName: test-ada
-Include: %shared %logger %test-utils %deathstack %xbee
-Defines: -DSTANDALONE_CATCH1_TEST -DDEATH_STACK_2 -DSDRAM_ISSI -DDEBUG
-Main: catch/fsm/test-ada
+BoardId: stm32f429zi_skyward_death_stack_x
+BinName: test-deployment
+Include: %shared %test-utils %pwm %servo %deployment %hbridge
+Defines: -DSTANDALONE_CATCH1_TEST
+Main: catch/fsm/test-deployment
+
+[test-deployment-interactive]
+Type: test
+BoardId: stm32f429zi_skyward_death_stack_x
+BinName: test-deployment-interactive
+Include: %shared %test-utils %pwm %servo %deployment %hbridge
+Defines: -DSTANDALONE_CATCH1_TEST -DDEBUG
+Main: deployment/test-deployment-interactive
+
+[test-aerobrakes]
+Type: test
+BoardId: stm32f429zi_skyward_death_stack_x
+BinName: test-aerobrakes
+Include: %shared %test-utils %pwm %servo
+Defines: -DSTANDALONE_CATCH1_TEST
+Main: catch/fsm/test-aerobrakes
+
+[test-aerobrakes-interactive]
+Type: test
+BoardId: stm32f429zi_skyward_death_stack_x
+BinName: test-aerobrakes-interactive
+Include: %shared %test-utils %pwm %servo
+Defines: -DDEBUG
+Main: aerobrakes/test-aerobrakes-interactive
+
+[test-aerobrakes-algorithm]
+Type: test
+BoardId: stm32f429zi_skyward_death_stack_x
+BinName: test-aerobrakes-algorithm
+Include: %shared %servo %pwm
+Defines: -DDEBUG
+Main: aerobrakes/test-aerobrakes-algorithm
diff --git a/scripts/event_header_generator/EventStrings.cpp.template b/scripts/event_header_generator/EventStrings.cpp.template
deleted file mode 100644
index d11c60bbcd8c6f9896aa2e36f18cd173e3f4eb31..0000000000000000000000000000000000000000
--- a/scripts/event_header_generator/EventStrings.cpp.template
+++ /dev/null
@@ -1,60 +0,0 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
- *
- * 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.
- */
-
-/*
- ******************************************************************************
- * THIS FILE IS AUTOGENERATED. DO NOT EDIT. *
- ******************************************************************************
- */
-
-// Generated from: {sheet_link}
-// Autogen date: {date}
-
-
-#include "Events.h"
-#include "Topics.h"
-
-#include <map>
-using std::map;
-
-namespace DeathStackBoard
-{{
-
-string getEventString(uint8_t event)
-{{
- static const map<uint8_t, string> event_string_map {{
-{event_map_data}
- }};
- auto it = event_string_map.find(event);
- return it == event_string_map.end() ? "EV_UNKNOWN" : it->second;
-}}
-
-string getTopicString(uint8_t topic)
-{{
- static const map<uint8_t, string> topic_string_map{{
-{topic_map_data}
- }};
- auto it = topic_string_map.find(topic);
- return it == topic_string_map.end() ? "TOPIC_UNKNOWN" : it->second;
-}}
-
-}}
\ No newline at end of file
diff --git a/scripts/event_header_generator/README.md b/scripts/event_header_generator/README.md
deleted file mode 100644
index f87e46c67ebcec406b02c58e10dbe91b00a0c8d3..0000000000000000000000000000000000000000
--- a/scripts/event_header_generator/README.md
+++ /dev/null
@@ -1,14 +0,0 @@
-# Events Generator Script
-
-This script generates the Events.h and Topics.h heander file from a GoogleSheets document on Google Drive.
-
-To execute the script:
-
-```
-pip install --upgrade google-api-python-client
-pip install oauth2client
-python scripts/homeone/event_header_generator/event_generator.py
-```
-
-The first time a browser window should open asking for your credentials (use the Skyward ones).
-If it doesn't and only gives you an error, try executing it by clicking on the icon.
\ No newline at end of file
diff --git a/scripts/event_header_generator/credentials.json b/scripts/event_header_generator/credentials.json
deleted file mode 100644
index dae318afc5661116b82fe75f80f4316e538e670a..0000000000000000000000000000000000000000
--- a/scripts/event_header_generator/credentials.json
+++ /dev/null
@@ -1 +0,0 @@
-{"installed":{"client_id":"1025168905991-tv31etsgm3lecodc5c798shqciekad40.apps.googleusercontent.com","project_id":"homeone-event-ge-1540834105298","auth_uri":"https://accounts.google.com/o/oauth2/auth","token_uri":"https://www.googleapis.com/oauth2/v3/token","auth_provider_x509_cert_url":"https://www.googleapis.com/oauth2/v1/certs","client_secret":"Yhaf67HuHR4DyXZKNXWu2lre","redirect_uris":["urn:ietf:wg:oauth:2.0:oob","http://localhost"]}}
\ No newline at end of file
diff --git a/scripts/event_header_generator/event_generator.py b/scripts/event_header_generator/event_generator.py
deleted file mode 100644
index 048a2ba7f8fe2725e46533b6877e974bf75e8cfc..0000000000000000000000000000000000000000
--- a/scripts/event_header_generator/event_generator.py
+++ /dev/null
@@ -1,209 +0,0 @@
-#!/usr/bin/python3
-
-# Copyright (c) 2018 Skyward Experimental Rocketry
-# Authors: Luca Erbetta
-#
-# 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.
-#
-
-from googleapiclient.discovery import build
-from httplib2 import Http
-from oauth2client import file, client, tools
-
-import re
-import datetime
-import sys
-from os.path import join
-import os
-
-OUTPUT_FOLDER = "generated"
-SCOPES = 'https://www.googleapis.com/auth/spreadsheets.readonly'
-
-service = None
-
-# Spreadsheet file used to generate the events. The can be found in the link
-# to the spreadsheet, for example:
-# https://docs.google.com/spreadsheets/d/184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0/
-# -->
-# 184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0
-SPREADSHEET_ID = '184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0'
-EVENTS_RANGE_NAME = 'EventList!A2:A'
-TOPICS_RANGE_NAME = 'Topics!B3:B'
-
-
-def auth():
- try:
- store = file.Storage('store.json')
- creds = store.get()
- if not creds or creds.invalid:
- flow = client.flow_from_clientsecrets('credentials.json', SCOPES)
- creds = tools.run_flow(flow, store)
-
- global service
- service = build('sheets', 'v4', http=creds.authorize(Http()))
- return True
- except:
- print("Authentication error:", sys.exc_info()[0])
- return False
-
-
-def load_events():
- result = service.spreadsheets().values().get(spreadsheetId=SPREADSHEET_ID,
- range=EVENTS_RANGE_NAME).execute()
- lines = result.get('values', [])
-
- # Event names start with EV_ and contains only uppercase letters or underscores
- re_event = re.compile(r'(?P<ev>^EV_([A-Z_]+)+)')
-
- # Only return lines with valid events, remove additional data
- events = []
- for l in lines:
- m = re_event.match(l[0])
- if m is not None:
- events.append(m.group('ev'))
- else:
- print("Skipped line containing invalid event: {}".format(l))
-
- return events
-
-
-def load_topics():
- result = service.spreadsheets().values().get(spreadsheetId=SPREADSHEET_ID,
- range=TOPICS_RANGE_NAME).execute()
- lines = result.get('values', [])
- re_topics = re.compile(r'(?P<topic>^TOPIC_([A-Z_]+)+)')
-
- # Only return lines with valid topics, remove additional data
- topics = []
- for l in lines:
- m = re_topics.match(l[0])
- if m is not None:
- topics.append(m.group('topic'))
- else:
- print("Skipped line containing invalid topics: {}".format(l))
-
- return topics
-
-
-def has_duplicates(lst):
- if len(lst) != len(set(lst)):
- return True
- return False
-
-
-
-print("Skyward on-board software event header generator v0.2")
-print("Google sheets API auth in progress...")
-
-if auth():
- print("Auth successfull.")
-else:
- exit()
-
-#directory = os.path.dirname(OUTPUT_FOLDER)
-if not os.path.exists(OUTPUT_FOLDER):
- os.mkdir(OUTPUT_FOLDER)
-
-print("Reading from: https://docs.google.com/spreadsheets/d/" + SPREADSHEET_ID)
-
-events = load_events()
-topics = load_topics()
-
-# Check duplicates
-if has_duplicates(events):
- print("Duplicate events found! Terminating.")
- exit()
-
-if has_duplicates(topics):
- print("Duplicate topics found! Terminating.")
- exit()
-
-events.sort()
-topics.sort()
-
-print("{} events loaded.".format(len(events)))
-print("{} topics loaded.".format(len(topics)))
-
-enum_str = ""
-event_map_str = ""
-event_list_str = ""
-
-date = datetime.datetime.now()
-link = "https://docs.google.com/spreadsheets/d/{id}".format(id=SPREADSHEET_ID)
-
-
-# Events.h generation
-
-print("Generating Events.h...")
-
-for e in events:
- endl = ",\n" if e != events[-1] else ""
- enum_str += " " + e + \
- (" = EV_FIRST_SIGNAL" if e == events[0] else "") + endl
- event_map_str += " {{ {event}, {string} }}{endl}".format(
- event=e, string='"' + e + '"', endl=endl)
- event_list_str += e + (", " if e != events[-1] else "")
-
-with open('Events.h.template', 'r') as template_file:
- template = template_file.read()
-
-template = template.format(sheet_link=link, date=date,
- enum_data=enum_str,event_list=event_list_str)
-
-with open(join(OUTPUT_FOLDER, 'Events.h'), 'w') as header_file:
- header_file.write(template)
-
-print("Events.h successfully generated.")
-# Topics.h generation
-
-print("Generating Topics.h...")
-enum_str = ""
-topic_map_str = ""
-topic_list_str = ""
-
-for t in topics:
- endl = ",\n" if t != topics[-1] else ""
- enum_str += " " + t + endl
- topic_map_str += " {{ {topics}, {string} }}{endl}".format(
- topics=t, string='"' + t + '"', endl=endl)
- topic_list_str += t + (", " if t != topics[-1] else "")
-
-with open('Topics.h.template', 'r') as template_file:
- template = template_file.read()
-
-template = template.format(sheet_link=link, date=date,
- enum_data=enum_str, topic_list=topic_list_str)
-
-with open(join(OUTPUT_FOLDER, 'Topics.h'), 'w') as header_file:
- header_file.write(template)
-
-with open('EventStrings.cpp.template', 'r') as cpp_template_file:
- cpp = cpp_template_file.read()
-
-cpp = cpp.format(sheet_link=link, date=date,
- event_map_data=event_map_str, topic_map_data=topic_map_str)
-
-with open(join(OUTPUT_FOLDER, 'EventStrings.cpp'), 'w') as cpp_file:
- cpp_file.write(cpp)
-
-print("Topics.h successfully generated.")
-
-print()
-print("All files successfully generated. Please move Events.h, Topics.h, EventStrings.cpp into your project sources.")
-print(".... Done.")
diff --git a/scripts/event_header_generator/requirements.txt b/scripts/event_header_generator/requirements.txt
deleted file mode 100644
index b4e01569861fa5001ba712bfb61688ed45a3f451..0000000000000000000000000000000000000000
--- a/scripts/event_header_generator/requirements.txt
+++ /dev/null
@@ -1,20 +0,0 @@
-cachetools==3.1.0
-certifi==2019.3.9
-chardet==3.0.4
-google-api-python-client==1.7.8
-google-auth==1.6.3
-google-auth-httplib2==0.0.3
-google-auth-oauthlib==0.3.0
-httplib2==0.12.3
-idna==2.8
-oauth2client==4.1.3
-oauthlib==3.0.1
-pkg-resources==0.0.0
-pyasn1==0.4.5
-pyasn1-modules==0.2.5
-requests==2.21.0
-requests-oauthlib==1.2.0
-rsa==4.0
-six==1.12.0
-uritemplate==3.0.0
-urllib3==1.24.2
diff --git a/scripts/eventgen.sh b/scripts/eventgen.sh
new file mode 100755
index 0000000000000000000000000000000000000000..6763addd5072948bc8e191ff4e61e4c31c20494a
--- /dev/null
+++ b/scripts/eventgen.sh
@@ -0,0 +1,25 @@
+#!/bin/bash
+
+# Copyright (c) 2021 Skyward Experimental Rocketry
+# Authors: Alberto Nidasio
+#
+# 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.
+
+DIRNAME="$(dirname $0)"
+$DIRNAME/../skyward-boardcore/scripts/generators/eventgen.py $@
diff --git a/scripts/fsmgen.sh b/scripts/fsmgen.sh
new file mode 100755
index 0000000000000000000000000000000000000000..2bae83d7f938ec78624784bfbb93aaa0f17e4431
--- /dev/null
+++ b/scripts/fsmgen.sh
@@ -0,0 +1,25 @@
+#!/bin/bash
+
+# Copyright (c) 2021 Skyward Experimental Rocketry
+# Authors: Alberto Nidasio
+#
+# 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.
+
+DIRNAME="$(dirname $0)"
+$DIRNAME/../skyward-boardcore/scripts/generators/fsmgen.py $@
diff --git a/scripts/linter.sh b/scripts/linter.sh
new file mode 100755
index 0000000000000000000000000000000000000000..f0b2eac4f3a6e8628c1abe05506886fe2b7fc572
--- /dev/null
+++ b/scripts/linter.sh
@@ -0,0 +1,4 @@
+#!/bin/bash
+
+DIRNAME="$(dirname $0)"
+$DIRNAME/../skyward-boardcore/scripts/linter.sh $DIRNAME/../src/boards
\ No newline at end of file
diff --git a/scripts/scxml2plant.sh b/scripts/scxml2plant.sh
new file mode 100755
index 0000000000000000000000000000000000000000..7a4d5a9cd20a8837ae09f940ff8b382d171ffee0
--- /dev/null
+++ b/scripts/scxml2plant.sh
@@ -0,0 +1,4 @@
+#!/bin/bash
+
+DIRNAME="$(dirname $0)"
+$DIRNAME/../skyward-boardcore/scripts/scxml2plant/scxml2plant.sh $DIRNAME/../src/boards
\ No newline at end of file
diff --git a/skyward-boardcore b/skyward-boardcore
index bc67604abf3ceaf08b8772803ddeb99c64696c15..41b1865e97ff6567c33e4c14beb065344611059f 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit bc67604abf3ceaf08b8772803ddeb99c64696c15
+Subproject commit 41b1865e97ff6567c33e4c14beb065344611059f
diff --git a/src/boards/DeathStack/ADA/ADA.cpp b/src/boards/DeathStack/ADA/ADA.cpp
index fb0a5d230abe8686df76534259bd55012c102bdf..8856f711b8b223854457aafff52d5a49c85bcc64 100644
--- a/src/boards/DeathStack/ADA/ADA.cpp
+++ b/src/boards/DeathStack/ADA/ADA.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Mozzarelli
+/* Copyright (c) 2018-2021 Skyward Experimental Rocketry
+ * Authors: Luca Mozzarelli, Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -20,30 +20,26 @@
* THE SOFTWARE.
*/
#include "ADA.h"
-#include <events/Events.h>
+
#include <Debug.h>
-#include <configs/ADA_config.h>
+#include <configs/ADAconfig.h>
#include <events/EventBroker.h>
-#include <libs/simple-template-matrix/matrix.h>
+#include <events/Events.h>
#include <utils/aero/AeroUtils.h>
+
#include "DeploymentUtils/elevation_map.h"
+#include "TimestampTimer.h"
+#include "diagnostic/CpuMeter.h"
namespace DeathStackBoard
{
-ADA::ADA(ReferenceValues ref_values)
- : filter(A_INIT, C_INIT, V1_INIT, V2_INIT, P_INIT),
- filter_acc(A_INIT, C_INIT_ACC, V1_INIT_ACC, V2_INIT_ACC, P_INIT_ACC),
- ref_values(ref_values)
-{
- // Initialize Kalman filter
- filter.X(0, 0) = ref_values.ref_pressure;
- filter.X(1, 0) = 0;
- filter.X(2, 0) = KALMAN_INITIAL_ACCELERATION;
- filter_acc.X(0, 0) = ref_values.ref_altitude;
- filter_acc.X(1, 0) = 0;
- filter_acc.X(2, 0) = 0;
+using namespace ADAConfigs;
+ADA::ADA(ReferenceValues ref_values)
+ : ref_values(ref_values),
+ filter(ADAConfigs::getKalmanConfig(ref_values.ref_pressure))
+{
TRACE("[ADA] Finalized calibration. p_ref: %.3f, p0: %.3f, t0: %.3f\n",
ref_values.ref_pressure, ref_values.msl_pressure,
ref_values.msl_temperature);
@@ -53,49 +49,26 @@ ADA::~ADA() {}
void ADA::updateBaro(float pressure)
{
- // First kalman (pressure only)
- MatrixBase<float, 1, 1> y{pressure};
- filter.update(y);
-
- // Second kalman (pressure and acceleration)
- float z = pressureToAltitude(pressure);
- float ax = last_acc_average;
-
- MatrixBase<float, 2, 1> y_acc{z, ax};
- filter_acc.update(y_acc);
+ updatePressureKalman(pressure);
// Convert filter data to altitudes & speeds
- ada_data.timestamp = miosix::getTick();
- ada_data.msl_altitude = pressureToAltitude(filter.X(0, 0));
+ ada_data.timestamp = TimestampTimer::getTimestamp();
+ ada_data.msl_altitude = pressureToAltitude(filter.getState()(0, 0));
AltitudeDPL ad = altitudeMSLtoDPL(ada_data.msl_altitude);
ada_data.dpl_altitude = ad.altitude;
ada_data.is_dpl_altitude_agl = ad.is_agl;
ada_data.vert_speed = aeroutils::verticalSpeed(
- filter.X(0, 0), filter.X(1, 0), ref_values.msl_pressure,
- ref_values.msl_temperature);
-
- // Filter with accelerometer
- ada_data.acc_msl_altitude = filter_acc.X(0, 0);
- ada_data.acc_vert_speed = filter_acc.X(1, 0);
-}
-
-void ADA::updateAcc(float ax)
-{
- acc_stats.add(ax - 9.81f);
- if (acc_stats.n_samples >= ACCELERATION_AVERAGE_N_SAMPLES)
- {
- last_acc_average = acc_stats.getAverage();
- acc_stats.reset();
- }
+ filter.getState()(0, 0), filter.getState()(1, 0),
+ ref_values.msl_pressure, ref_values.msl_temperature);
}
-void ADA::updateGPS(double lat, double lon, bool has_fix)
+void ADA::updateGPS(float lat, float lon, bool fix)
{
last_lat = lat;
last_lon = lon;
- last_fix = has_fix;
+ last_fix = fix;
}
float ADA::getAltitudeMsl() const { return ada_data.msl_altitude; }
@@ -107,7 +80,7 @@ ADA::AltitudeDPL ADA::getAltitudeForDeployment() const
float ADA::getVerticalSpeed() const { return ada_data.vert_speed; }
-float ADA::pressureToAltitude(float pressure) const
+float ADA::pressureToAltitude(float pressure)
{
return aeroutils::relAltitude(pressure, ref_values.msl_pressure,
ref_values.msl_temperature);
@@ -127,19 +100,27 @@ ADA::AltitudeDPL ADA::altitudeMSLtoDPL(float altitude_msl) const
}
}
-KalmanState ADA::getKalmanState() const
+ADAKalmanState ADA::getKalmanState()
{
- KalmanState state;
- state.timestamp = miosix::getTick();
-
- state.x0 = filter.X(0, 0);
- state.x1 = filter.X(1, 0);
- state.x2 = filter.X(2, 0);
+ ADAKalmanState state;
+ state.timestamp = TimestampTimer::getTimestamp();
- state.x0_acc = filter_acc.X(0, 0);
- state.x1_acc = filter_acc.X(1, 0);
- state.x2_acc = filter_acc.X(2, 0);
+ state.x0 = filter.getState()(0, 0);
+ state.x1 = filter.getState()(1, 0);
+ state.x2 = filter.getState()(2, 0);
return state;
}
+
+void ADA::updatePressureKalman(float pressure)
+{
+ filter.predict();
+
+ CVectorP y(pressure); // column vector
+ if (!filter.correct(y))
+ {
+ TRACE("[ADA] Kalman correction step failed \n");
+ }
+}
+
} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/ADA/ADA.h b/src/boards/DeathStack/ADA/ADA.h
index da7368924231c53f0377e993086d2ac92cd4d4ea..3119816872317639caa279b4e30545fea8779e84 100644
--- a/src/boards/DeathStack/ADA/ADA.h
+++ b/src/boards/DeathStack/ADA/ADA.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Mozzarelli
+/* Copyright (c) 2018-2021 Skyward Experimental Rocketry
+ * Authors: Luca Mozzarelli, Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -22,12 +22,15 @@
#pragma once
-#include <kalman/Kalman.h>
+#include <kalman/KalmanEigen.h>
#include <math/Stats.h>
+
#include "ADAStatus.h"
+#include "sensors/Sensor.h"
namespace DeathStackBoard
{
+
class ADA
{
public:
@@ -37,14 +40,15 @@ public:
bool is_agl;
};
- ADA(ReferenceValues setup_data);
+ ADA(ReferenceValues ref_values);
+
~ADA();
void updateBaro(float pressure);
- void updateAcc(float ax);
- void updateGPS(double lat, double lon, bool has_fix);
- KalmanState getKalmanState() const;
+ void updateGPS(float lat, float lon, bool fix);
+
+ ADAKalmanState getKalmanState();
ADAData getADAData() const { return ada_data; }
@@ -77,7 +81,7 @@ public:
* @param pressure Atmospheric pressure in Pa
* @return Corresponding altitude above mean sea level (m)
*/
- float pressureToAltitude(float pressure) const;
+ float pressureToAltitude(float pressure);
/**
* @brief Converts an altitude above mean sea level to altitude for chute
@@ -89,39 +93,17 @@ public:
ReferenceValues getReferenceValues() const { return ref_values; }
private:
- Kalman<3, 1> filter; // Filter object
- Kalman<3, 2> filter_acc; // Filter with accelerometer
+ void updatePressureKalman(float pressure);
// References for pressure to altitude conversion
ReferenceValues ref_values;
- ADAData ada_data;
+ KalmanEigen<float, KALMAN_STATES_NUM, KALMAN_OUTPUTS_NUM> filter;
- struct AccAverage
- {
- float accumulator = 0;
- unsigned int n_samples = 0;
-
- void add(float acc)
- {
- accumulator += acc;
- n_samples++;
- }
-
- float getAverage() { return accumulator / n_samples; }
-
- void reset()
- {
- n_samples = 0;
- accumulator = 0;
- }
- };
-
- float last_acc_average = 0;
- AccAverage acc_stats;
+ ADAData ada_data;
- double last_lat = 0;
- double last_lon = 0;
- bool last_fix = false;
+ float last_lat = 0;
+ float last_lon = 0;
+ bool last_fix = false;
};
} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/ADA/ADACalibrator.cpp b/src/boards/DeathStack/ADA/ADACalibrator.cpp
index 41e0d460bca3942529df38d2e214e59abd883429..c07593e18861c87916f1356fc88cbc9e329301e0 100644
--- a/src/boards/DeathStack/ADA/ADACalibrator.cpp
+++ b/src/boards/DeathStack/ADA/ADACalibrator.cpp
@@ -21,16 +21,22 @@
*/
#include "ADACalibrator.h"
+
#include <utils/aero/AeroUtils.h>
namespace DeathStackBoard
{
+using namespace ADAConfigs;
+
void ADACalibrator::addBaroSample(float p) { pressure_stats.add(p); }
bool ADACalibrator::calibIsComplete()
{
// Calibration is complete if enough samples were collected and reference
// altitude and temperature were set
+
+ //TRACE("Calibrator : n samples = %d \n", pressure_stats.getStats().nSamples);
+
return pressure_stats.getStats().nSamples >= CALIBRATION_BARO_N_SAMPLES &&
ref_alt_set && ref_temp_set;
}
diff --git a/src/boards/DeathStack/ADA/ADACalibrator.h b/src/boards/DeathStack/ADA/ADACalibrator.h
index e6b04a2ca0a3c0c7617e1aceebc03cef57fb55c1..63f7a060ba0b5153cf974ee8c36237816f96d2c6 100644
--- a/src/boards/DeathStack/ADA/ADACalibrator.h
+++ b/src/boards/DeathStack/ADA/ADACalibrator.h
@@ -26,28 +26,20 @@
#include <Debug.h>
#include <math/Stats.h>
#include <miosix.h>
+
#include "ADAStatus.h"
namespace DeathStackBoard
{
class ADACalibrator
{
-private:
- /* --- CALIBRATION --- */
- ReferenceValues ref_values{};
- Stats pressure_stats; // Computes mean std dev etc for calibration of
- // pressure conversion
-
- // Refernece flags
- bool ref_alt_set = false;
- bool ref_temp_set = false;
public:
/* --- CALIBRATION --- */
ReferenceValues getReferenceValues();
bool calibIsComplete();
- void addBaroSample(float p); // Adds a pressure sample to the stats
- void resetBaro(); // Resets only pressure stats
+ void addBaroSample(float p); // Adds a pressure sample to the stats
+ void resetBaro(); // Resets only pressure stats
/* --- TC ---*/
/**
@@ -61,5 +53,16 @@ public:
* @param ref_alt Reference altitude in meters above mean sea level
*/
void setReferenceAltitude(float ref_alt);
+
+private:
+ /* --- CALIBRATION --- */
+ ReferenceValues ref_values{};
+
+ Stats pressure_stats; // Computes mean std dev etc for calibration of
+ // pressure conversion
+
+ // Refernece flags
+ bool ref_alt_set = false;
+ bool ref_temp_set = false;
};
} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/ADA/ADAController.cpp b/src/boards/DeathStack/ADA/ADAController.cpp
deleted file mode 100644
index 558052855a684a4d2eeb0401edbfff7280b37d18..0000000000000000000000000000000000000000
--- a/src/boards/DeathStack/ADA/ADAController.cpp
+++ /dev/null
@@ -1,617 +0,0 @@
-/* Copyright (c) 2018,2019 Skyward Experimental Rocketry
- * Authors: Luca Mozzarelli, Luca Erbetta
- *
- * 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 <ADA/ADAController.h>
-#include <events/EventBroker.h>
-#include <utils/aero/AeroUtils.h>
-#include "System/StackLogger.h"
-#include "Debug.h"
-
-using miosix::Lock;
-
-namespace DeathStackBoard
-{
-
-/* --- LIFE CYCLE --- */
-
-ADAController::ADAController()
- : FSM(&ADAController::stateIdle, 4096, 2), ada(ReferenceValues{})
-{
- // Subscribe to topics
- sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS);
- sEventBroker->subscribe(this, TOPIC_TC);
- sEventBroker->subscribe(this, TOPIC_ADA);
-
- status.state = ADAState::IDLE;
-}
-
-/* --- SENSOR UPDATE METHODS --- */
-
-void ADAController::updateGPS(double lat, double lon, bool hasFix)
-{
- // Update gps regardless of the current state
- ada.updateGPS(lat, lon, hasFix);
-}
-
-void ADAController::updateBaro(float pressure)
-{
- ADAState state = status.state;
-
- switch (state)
- {
- case ADAState::IDLE:
- {
- break;
- }
- case ADAState::CALIBRATING:
- {
- bool end_calib = false;
- {
- Lock<FastMutex> l(calibrator_mutex);
-
- // Add samples to the calibration
- calibrator.addBaroSample(pressure);
-
- // Save the state of calibration to release mutex
- end_calib = calibrator.calibIsComplete();
- }
-
- if (end_calib)
- {
- // If samples are enough and dpl altitude has been set init ada
- finalizeCalibration();
- }
- break;
- }
-
- case ADAState::READY:
- {
- // Log the altitude & vertical speed but don't use kalman pressure
- // while we are on the ramp
- ADAData d;
- d.timestamp = miosix::getTick();
-
- d.msl_altitude = ada.pressureToAltitude(pressure);
- d.vert_speed = 0;
-
- d.acc_msl_altitude = 0;
- d.acc_vert_speed = 0;
-
- ADA::AltitudeDPL ad = ada.altitudeMSLtoDPL(d.msl_altitude);
- d.dpl_altitude = ad.altitude;
- d.is_dpl_altitude_agl = ad.is_agl;
-
- logger.log(d);
- break;
- }
-
- case ADAState::SHADOW_MODE:
- {
- // Shadow mode state: update kalman, DO NOT send events
- ada.updateBaro(pressure);
-
- // Check if the vertical speed smaller than the target apogee speed
- if (ada.getVerticalSpeed() < APOGEE_VERTICAL_SPEED_TARGET)
- {
- // Log
- ApogeeDetected apogee{status.state, miosix::getTick()};
- logger.log(apogee);
- }
-
- logData(ada.getKalmanState(), ada.getADAData());
-
- break;
- }
-
- case ADAState::ACTIVE:
- {
- ada.updateBaro(pressure);
- // Check if we reached apogee
- if (ada.getVerticalSpeed() < APOGEE_VERTICAL_SPEED_TARGET)
- {
- if (++n_samples_apogee_detected >= APOGEE_N_SAMPLES)
- {
- // Active state send notifications for apogee
- sEventBroker->post({EV_ADA_APOGEE_DETECTED}, TOPIC_ADA);
- status.apogee_reached = true;
- }
-
- // Log
- ApogeeDetected apogee{status.state, miosix::getTick()};
- logger.log(apogee);
- }
- else if (n_samples_apogee_detected != 0)
- {
- n_samples_apogee_detected = 0;
- }
-
- logData(ada.getKalmanState(), ada.getADAData());
- break;
- }
-
- case ADAState::PRESSURE_STABILIZATION:
- {
- // Stabilization state: do not send notifications for target
- // altitude reached, log it
- ada.updateBaro(pressure);
-
- if (ada.getAltitudeForDeployment().altitude <=
- deployment_altitude &&
- ada.getAltitudeMsl() <= MAX_DEPLOYMENT_ALTITUDE_MSL)
- {
- if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES)
- {
- logger.log(DplAltitudeReached{miosix::getTick()});
- }
- }
- else if (n_samples_deployment_detected != 0)
- {
- n_samples_deployment_detected = 0;
- }
-
- logData(ada.getKalmanState(), ada.getADAData());
- break;
- }
-
- case ADAState::FIRST_DESCENT_PHASE:
- {
- // Descent state: send notifications for target altitude reached
- ada.updateBaro(pressure);
-
- if (ada.getAltitudeForDeployment().altitude <=
- deployment_altitude &&
- ada.getAltitudeMsl() <= MAX_DEPLOYMENT_ALTITUDE_MSL)
- {
- if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES)
- {
- logger.log(DplAltitudeReached{miosix::getTick()});
-
- sEventBroker->post({EV_ADA_DPL_ALT_DETECTED}, TOPIC_ADA);
- }
- }
- else if (n_samples_deployment_detected != 0)
- {
- n_samples_deployment_detected = 0;
- }
-
- logData(ada.getKalmanState(), ada.getADAData());
- break;
- }
- case ADAState::END:
- {
- // Continue updating the filter for logging & telemetry purposes
- ada.updateBaro(pressure);
-
- logData(ada.getKalmanState(), ada.getADAData());
- break;
- }
- case ADAState::UNDEFINED:
- {
- TRACE("[ADA] Update Baro: Undefined state value \n");
- break;
- }
-
- default:
- {
- TRACE("[ADA] Update Baro: Unexpected state value \n");
- break;
- }
- }
-}
-
-void ADAController::updateAcc(float ax)
-{
- // Update acceleration unconditionally
- ada.updateAcc(ax);
-}
-
-/* --- TC --- */
-void ADAController::setReferenceTemperature(float ref_temp)
-{
- if (status.state == ADAState::CALIBRATING ||
- status.state == ADAState::READY)
- {
- {
- Lock<FastMutex> l(calibrator_mutex);
- calibrator.setReferenceTemperature(ref_temp);
- logger.log(calibrator.getReferenceValues());
- }
-
- finalizeCalibration();
- }
-}
-
-void ADAController::setReferenceAltitude(float ref_alt)
-{
- if (status.state == ADAState::CALIBRATING ||
- status.state == ADAState::READY)
- {
- {
- Lock<FastMutex> l(calibrator_mutex);
- calibrator.setReferenceAltitude(ref_alt);
- logger.log(calibrator.getReferenceValues());
- }
-
- finalizeCalibration();
- }
-}
-
-void ADAController::setDeploymentAltitude(float dpl_alt)
-{
- if (status.state == ADAState::CALIBRATING ||
- status.state == ADAState::READY)
- {
- {
- Lock<FastMutex> l(calibrator_mutex);
-
- deployment_altitude = dpl_alt;
- deployment_altitude_set = true;
- }
- logger.log(TargetDeploymentAltitude{dpl_alt});
-
- TRACE("[ADA] Deployment altitude set to %.3f m\n", dpl_alt);
-
- finalizeCalibration();
- }
-}
-
-/* --- CALIBRATION --- */
-void ADAController::finalizeCalibration()
-{
- Lock<FastMutex> l(calibrator_mutex);
-
- if (calibrator.calibIsComplete() && deployment_altitude_set &&
- ada.getReferenceValues() != calibrator.getReferenceValues())
- {
- // If samples are enough and dpl altitude has been set init ada
- ada = ADA{calibrator.getReferenceValues()};
-
- // ADA READY!
- sEventBroker->post({EV_ADA_READY}, TOPIC_ADA);
-
- logger.log(calibrator.getReferenceValues());
- logger.log(ada.getKalmanState());
- }
-}
-
-/* --- STATES --- */
-/**
- * \brief Idle state: the ADA waits for a command to start calibration. This is
- * the initial state.
- */
-void ADAController::stateIdle(const Event& ev)
-{
- switch (ev.sig)
- {
- case EV_ENTRY:
- {
- TRACE("[ADA] Entering stateIdle\n");
- logStatus(ADAState::IDLE);
- break;
- }
- case EV_EXIT:
- {
- TRACE("[ADA] Exiting stateIdle\n");
- break;
- }
- case EV_CALIBRATE_ADA:
- {
- transition(&ADAController::stateCalibrating);
- break;
- }
- default:
- {
- // TRACE("[ADA] stateIdle: %d event not handled\n", ev.sig);
- break;
- }
- }
-}
-
-/**
- * \brief Calibrating state: the ADA calibrates the initial Kalman state.
- *
- * In this state a call to update() will result in a altitude sample being added
- * to the average.
- * The exiting transition to the idle state is triggered at the first sample
- * update after having set the deployment altitude and having reached the
- * minimum number of calibration samples.
- */
-void ADAController::stateCalibrating(const Event& ev)
-{
- switch (ev.sig)
- {
- case EV_ENTRY:
- {
- {
- Lock<FastMutex> l(calibrator_mutex);
- calibrator.resetBaro();
- }
- logStatus(ADAState::CALIBRATING);
- TRACE("[ADA] Entering stateCalibrating\n");
- break;
- }
- case EV_EXIT:
- {
- TRACE("[ADA] Exiting stateCalibrating\n");
- break;
- }
- case EV_ADA_READY:
- {
- transition(&ADAController::stateReady);
- break;
- }
- case EV_TC_CALIBRATE_ADA:
- {
- transition(&ADAController::stateCalibrating);
- break;
- }
- default:
- {
- // TRACE("ADA stateCalibrating: %d event not handled\n", ev.sig);
- break;
- }
- }
-}
-
-/**
- * \brief Ready 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 ADAController::stateReady(const Event& ev)
-{
- switch (ev.sig)
- {
- case EV_ENTRY:
- {
- logStatus(ADAState::READY);
- TRACE("[ADA] Entering stateReady\n");
- break;
- }
- case EV_EXIT:
- {
- TRACE("[ADA] Exiting stateReady\n");
- break;
- }
- case EV_LIFTOFF:
- {
- transition(&ADAController::stateShadowMode);
- break;
- }
- case EV_TC_CALIBRATE_ADA:
- {
- transition(&ADAController::stateCalibrating);
- break;
- }
- default:
- {
- // TRACE("ADA stateIdle: %d event not handled\n", 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 ADAController::stateShadowMode(const Event& ev)
-{
- switch (ev.sig)
- {
- case EV_ENTRY:
- {
- shadow_delayed_event_id =
- sEventBroker->postDelayed<TIMEOUT_ADA_SHADOW_MODE>(
- {EV_TIMEOUT_SHADOW_MODE}, TOPIC_ADA);
- logStatus(ADAState::SHADOW_MODE);
- TRACE("[ADA] Entering stateShadowMode\n");
- break;
- }
- case EV_EXIT:
- {
- sEventBroker->removeDelayed(shadow_delayed_event_id);
- TRACE("[ADA] Exiting stateShadowMode\n");
- break;
- }
- case EV_TIMEOUT_SHADOW_MODE:
- {
- transition(&ADAController::stateActive);
- break;
- }
- default:
- {
- // TRACE("ADA stateShadowMode: %d event not handled\n", 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 ADAController::stateActive(const Event& ev)
-{
- switch (ev.sig)
- {
- case EV_ENTRY:
- {
- logStatus(ADAState::ACTIVE);
- TRACE("[ADA] Entering stateActive\n");
- break;
- }
- case EV_EXIT:
- {
- TRACE("[ADA] Exiting stateActive\n");
- break;
- }
- case EV_ADA_APOGEE_DETECTED:
- {
- transition(&ADAController::statePressureStabilization);
- break;
- }
- default:
- {
- // TRACE("ADA stateActive: %d event not handled\n", ev.sig);
- break;
- }
- }
-}
-
-void ADAController::statePressureStabilization(const Event& ev)
-{
- switch (ev.sig)
- {
- case EV_ENTRY:
- {
- pressure_delayed_event_id =
- sEventBroker->postDelayed<TIMEOUT_ADA_P_STABILIZATION>(
- {EV_TIMEOUT_PRESS_STABILIZATION}, TOPIC_ADA);
- logStatus(ADAState::PRESSURE_STABILIZATION);
- TRACE("[ADA] Entering statePressureStabilization\n");
- break;
- }
- case EV_EXIT:
- {
- sEventBroker->removeDelayed(pressure_delayed_event_id);
- TRACE("[ADA] Exiting statePressureStabilization\n");
- break;
- }
- case EV_TIMEOUT_PRESS_STABILIZATION:
- {
- transition(&ADAController::stateFirstDescentPhase);
- break;
- }
- default:
- {
- // TRACE("ADA statePressureStabilization: %d event not handled\n",
- // ev.sig);
- break;
- }
- }
-}
-
-/**
- * \brief First descent phase state: ADA is running and it generates an event
- * when a set 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 altitude.
- * The exiting transition to the stop state is triggered by the parachute
- * deployment altitude reached event (NOT self generated!)
- */
-void ADAController::stateFirstDescentPhase(const Event& ev)
-{
- switch (ev.sig)
- {
- case EV_ENTRY:
- {
- logStatus(ADAState::FIRST_DESCENT_PHASE);
- TRACE("[ADA] Entering stateFirstDescentPhase\n");
- n_samples_deployment_detected = 0;
- break;
- }
- case EV_EXIT:
- {
- TRACE("[ADA] Exiting stateFirstDescentPhase\n");
- break;
- }
- case EV_ADA_DPL_ALT_DETECTED:
- {
- status.dpl_altitude_reached = true;
- logStatus();
-
- transition(&ADAController::stateEnd);
- break;
- }
- default:
- {
- // TRACE("ADA stateFirstDescentPhase: %d event not handled\n",
- // 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 ADAController::stateEnd(const Event& ev)
-{
- switch (ev.sig)
- {
- case EV_ENTRY:
- {
- TRACE("[ADA] Entering stateEnd\n");
- logStatus(ADAState::END);
- break;
- }
- case EV_EXIT:
- {
- TRACE("[ADA] Exiting stateEnd\n");
- break;
- }
- default:
- {
- // TRACE("ADA stateEnd: %d event not handled\n", ev.sig);
- break;
- }
- }
-}
-
-/* --- LOGGER --- */
-void ADAController::logStatus(ADAState state)
-{
- status.state = state;
- logStatus();
-}
-
-void ADAController::logStatus()
-{
- status.timestamp = miosix::getTick();
- logger.log(status);
-
- StackLogger::getInstance()->updateStack(THID_ADA_FSM);
-}
-
-void ADAController::logData(const KalmanState& s, const ADAData& d)
-{
- logger.log(s);
- logger.log(d);
-}
-
-} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/ADA/ADAController.h b/src/boards/DeathStack/ADA/ADAController.h
index fd9c7e54c6cfcf3d8b0f7a3bd1e867876c0e7713..b24ebe4f2f0a968bc879e7d9e9d4ae8c0038ff41 100644
--- a/src/boards/DeathStack/ADA/ADAController.h
+++ b/src/boards/DeathStack/ADA/ADAController.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Mozzarelli
+/* Copyright (c) 2018-2021 Skyward Experimental Rocketry
+ * Authors: Luca Mozzarelli, Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -23,16 +23,20 @@
#pragma once
#include <ADA/ADAStatus.h>
+#include <configs/ADAconfig.h>
+#include <events/EventBroker.h>
+#include <events/Events.h>
#include <events/FSM.h>
-#include "ADA.h"
-#include "ADACalibrator.h"
+#include <miosix.h>
+#include <utils/aero/AeroUtils.h>
-#include <configs/ADA_config.h>
-#include <events/Events.h>
-#include <kalman/Kalman.h>
+#include "ADA/ADA.h"
+#include "ADA/ADACalibrator.h"
+#include "Debug.h"
#include "LoggerService/LoggerService.h"
-
-#include <miosix.h>
+#include "System/StackLogger.h"
+#include "TimestampTimer.h"
+#include "sensors/SensorData.h"
using miosix::FastMutex;
using miosix::Lock;
@@ -40,22 +44,32 @@ using miosix::Lock;
namespace DeathStackBoard
{
-class ADAController : public FSM<ADAController>
+using namespace ADAConfigs;
+
+template <typename Press, typename GPS>
+class ADAController : public FSM<ADAController<Press, GPS>>
{
+ using ADACtrl = ADAController<Press, GPS>;
+ using ADAFsm = FSM<ADAController<Press, GPS>>;
+
+ static_assert(
+ checkIfProduces<Sensor<Press>, PressureData>::value,
+ "Template argument must be a sensor that produces pressure data.");
+ static_assert(checkIfProduces<Sensor<GPS>, GPSData>::value,
+ "Template argument must be a sensor that produces GPS data.");
public:
- ADAController();
- ~ADAController() {}
+ ADAController(Sensor<Press>& barometer, Sensor<GPS>& gps);
+
+ ~ADAController();
/* --- SENSOR UPDATE METHODS --- */
/*
* It's critical that this methods are called at regualar intervals during
- * the flight. Call frequency is defined in ADA_config.h The behavior of
- * this functions changes depending on the ADA state
+ * the flight. Call frequency is defined in configs/ADAconfig.h The behavior
+ * of this functions changes depending on the ADA state
*/
- void updateBaro(float pressure);
- void updateGPS(double lat, double lon, bool hasFix);
- void updateAcc(float ax);
+ void update();
/* --- TC --- */
/**
@@ -85,21 +99,24 @@ public:
private:
/* --- FSM STATES --- */
- void stateIdle(const Event& ev);
- void stateCalibrating(const Event& ev);
- void stateReady(const Event& ev);
- void stateShadowMode(const Event& ev);
- void stateActive(const Event& ev);
- void statePressureStabilization(const Event& ev);
- void stateFirstDescentPhase(const Event& ev);
- void stateEnd(const Event& ev);
+ void state_init(const Event& ev);
+ void state_idle(const Event& ev);
+ void state_calibrating(const Event& ev);
+ void state_ready(const Event& ev);
+ void state_shadowMode(const Event& ev);
+ void state_active(const Event& ev);
+ void state_pressureStabilization(const Event& ev);
+ void state_drogueDescent(const Event& ev);
+ void state_end(const Event& ev);
+
+ void updateBaroAccordingToState(float pressure);
void finalizeCalibration();
void logStatus(ADAState state); // Update and log ADA FSM state
void logStatus(); // Log the ADA FSM state without updating it
- void logData(const KalmanState& s, const ADAData& d);
+ void logData(const ADAKalmanState& s, const ADAData& d);
uint16_t shadow_delayed_event_id =
0; // Event id to store shadow mode timeout
@@ -116,19 +133,642 @@ private:
/* --- ALGORITHM --- */
ADA ada;
- float deployment_altitude = DEFAULT_DEPLOYMENT_ALTITUDE;
- bool deployment_altitude_set = false;
+ Sensor<Press>& barometer;
+ Sensor<GPS>& gps;
- // Number of consecutive samples in which apogee is detected
- unsigned int n_samples_apogee_detected = 0;
+ uint64_t last_press_timestamp = 0;
+ uint64_t last_gps_timestamp = 0;
- // Number of consecutive samples in which dpl altitude is detected
- unsigned int n_samples_deployment_detected = 0;
+ float deployment_altitude = DEFAULT_DEPLOYMENT_ALTITUDE;
+ bool deployment_altitude_set = false;
+
+ unsigned int n_samples_apogee_detected =
+ 0; /**< Number of consecutive samples in which apogee is detected */
+ unsigned int n_samples_deployment_detected =
+ 0; /**< Number of consecutive samples in which dpl altitude is detected
+ */
- /* --- LOGGER --- */
LoggerService& logger = *(LoggerService::getInstance()); // Logger
+};
+/* --- LIFE CYCLE --- */
+template <typename Press, typename GPS>
+ADAController<Press, GPS>::ADAController(Sensor<Press>& barometer,
+ Sensor<GPS>& gps)
+ : ADAFsm(&ADACtrl::state_idle, ADA_STACK_SIZE, ADA_PRIORITY),
+ ada(ReferenceValues{}), barometer(barometer), gps(gps)
+{
+ // Subscribe to topics
+ sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS);
+ sEventBroker->subscribe(this, TOPIC_ADA);
-};
+ status.state = ADAState::IDLE;
+}
+
+template <typename Press, typename GPS>
+ADAController<Press, GPS>::~ADAController()
+{
+}
+
+/* --- SENSOR UPDATE METHODS --- */
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::update()
+{
+ // if new gps data available, update GPS, regardless of the current state
+ GPS gps_data = gps.getLastSample();
+ if (gps_data.gps_timestamp > last_gps_timestamp)
+ {
+ last_gps_timestamp = gps_data.gps_timestamp;
+
+ ada.updateGPS(gps_data.latitude, gps_data.longitude, gps_data.fix);
+ }
+
+ // if new pressure data available, update baro, according to current state
+ Press press_data = barometer.getLastSample();
+
+ if (press_data.press_timestamp > last_press_timestamp)
+ {
+ last_press_timestamp = press_data.press_timestamp;
+
+ updateBaroAccordingToState(press_data.press);
+ }
+}
+
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure)
+{
+ ADAState state = status.state;
+
+ switch (state)
+ {
+ case ADAState::IDLE:
+ {
+ break;
+ }
+ case ADAState::CALIBRATING:
+ {
+ bool end_calib = false;
+ {
+ Lock<FastMutex> l(calibrator_mutex);
+
+ // Add samples to the calibration
+ calibrator.addBaroSample(pressure);
+
+ // Save the state of calibration to release mutex
+ end_calib = calibrator.calibIsComplete();
+ }
+
+ if (end_calib)
+ {
+ // If samples are enough and dpl altitude has been set init ada
+ finalizeCalibration();
+ }
+ break;
+ }
+
+ case ADAState::READY:
+ {
+ // Log the altitude & vertical speed but don't use kalman pressure
+ // while we are on the ramp
+ ADAData d;
+ d.timestamp = TimestampTimer::getTimestamp();
+
+ d.msl_altitude = ada.pressureToAltitude(pressure);
+ d.vert_speed = 0;
+
+ ADA::AltitudeDPL ad = ada.altitudeMSLtoDPL(d.msl_altitude);
+ d.dpl_altitude = ad.altitude;
+ d.is_dpl_altitude_agl = ad.is_agl;
+
+ logger.log(d);
+ break;
+ }
+
+ case ADAState::SHADOW_MODE:
+ {
+ // Shadow mode state: update kalman, DO NOT send events
+ ada.updateBaro(pressure);
+
+ // Check if the vertical speed smaller than the target apogee speed
+ if (ada.getVerticalSpeed() < APOGEE_VERTICAL_SPEED_TARGET)
+ {
+ // Log
+ ApogeeDetected apogee{status.state,
+ TimestampTimer::getTimestamp()};
+ logger.log(apogee);
+ }
+
+ logData(ada.getKalmanState(), ada.getADAData());
+
+ break;
+ }
+
+ case ADAState::ACTIVE:
+ {
+ ada.updateBaro(pressure);
+ // Check if we reached apogee
+ if (ada.getVerticalSpeed() < APOGEE_VERTICAL_SPEED_TARGET)
+ {
+ if (++n_samples_apogee_detected >= APOGEE_N_SAMPLES)
+ {
+ // Active state send notifications for apogee
+ sEventBroker->post({EV_ADA_APOGEE_DETECTED}, TOPIC_ADA);
+ status.apogee_reached = true;
+ }
+
+ // Log
+ ApogeeDetected apogee{status.state,
+ TimestampTimer::getTimestamp()};
+ logger.log(apogee);
+ }
+ else if (n_samples_apogee_detected != 0)
+ {
+ n_samples_apogee_detected = 0;
+ }
+
+ logData(ada.getKalmanState(), ada.getADAData());
+ break;
+ }
+
+ case ADAState::PRESSURE_STABILIZATION:
+ {
+ // Stabilization state: do not send notifications for target
+ // altitude reached, log it
+ ada.updateBaro(pressure);
+
+ if (ada.getAltitudeForDeployment().altitude <=
+ deployment_altitude &&
+ ada.getAltitudeMsl() <= MAX_DEPLOYMENT_ALTITUDE_MSL)
+ {
+ if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES)
+ {
+ logger.log(
+ DplAltitudeReached{TimestampTimer::getTimestamp()});
+ }
+ }
+ else if (n_samples_deployment_detected != 0)
+ {
+ n_samples_deployment_detected = 0;
+ }
+
+ logData(ada.getKalmanState(), ada.getADAData());
+ break;
+ }
+
+ case ADAState::DROGUE_DESCENT:
+ {
+ // Descent state: send notifications for target altitude reached
+ ada.updateBaro(pressure);
+
+ if (ada.getAltitudeForDeployment().altitude <=
+ deployment_altitude &&
+ ada.getAltitudeMsl() <= MAX_DEPLOYMENT_ALTITUDE_MSL)
+ {
+ if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES)
+ {
+ logger.log(
+ DplAltitudeReached{TimestampTimer::getTimestamp()});
+
+ sEventBroker->post({EV_ADA_DPL_ALT_DETECTED}, TOPIC_ADA);
+ }
+ }
+ else if (n_samples_deployment_detected != 0)
+ {
+ n_samples_deployment_detected = 0;
+ }
+
+ logData(ada.getKalmanState(), ada.getADAData());
+ break;
+ }
+ case ADAState::END:
+ {
+ // Continue updating the filter for logging & telemetry purposes
+ ada.updateBaro(pressure);
+
+ logData(ada.getKalmanState(), ada.getADAData());
+ break;
+ }
+ case ADAState::UNDEFINED:
+ {
+ TRACE("[ADA] Update Baro: Undefined state value \n");
+ break;
+ }
+
+ default:
+ {
+ TRACE("[ADA] Update Baro: Unexpected state value \n");
+ break;
+ }
+ }
+}
+
+/* --- TC --- */
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::setReferenceTemperature(float ref_temp)
+{
+ if (status.state == ADAState::CALIBRATING ||
+ status.state == ADAState::READY)
+ {
+ {
+ Lock<FastMutex> l(calibrator_mutex);
+ calibrator.setReferenceTemperature(ref_temp);
+ logger.log(calibrator.getReferenceValues());
+ }
+
+ finalizeCalibration();
+ }
+}
+
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::setReferenceAltitude(float ref_alt)
+{
+ if (status.state == ADAState::CALIBRATING ||
+ status.state == ADAState::READY)
+ {
+ {
+ Lock<FastMutex> l(calibrator_mutex);
+ calibrator.setReferenceAltitude(ref_alt);
+ logger.log(calibrator.getReferenceValues());
+ }
+
+ finalizeCalibration();
+ }
+}
+
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::setDeploymentAltitude(float dpl_alt)
+{
+ if (status.state == ADAState::CALIBRATING ||
+ status.state == ADAState::READY)
+ {
+ {
+ Lock<FastMutex> l(calibrator_mutex);
+
+ deployment_altitude = dpl_alt;
+ deployment_altitude_set = true;
+ }
+ logger.log(TargetDeploymentAltitude{dpl_alt});
+
+ TRACE("[ADA] Deployment altitude set to %.3f m\n", dpl_alt);
+
+ finalizeCalibration();
+ }
+}
+
+/* --- CALIBRATION --- */
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::finalizeCalibration()
+{
+ Lock<FastMutex> l(calibrator_mutex);
+
+ if (calibrator.calibIsComplete() && deployment_altitude_set &&
+ ada.getReferenceValues() != calibrator.getReferenceValues())
+ {
+ // If samples are enough and dpl altitude has been set init ada
+ ada = ADA{calibrator.getReferenceValues()};
+
+ TRACE("[ADAController] Finalized calibration \n");
+
+ // ADA READY!
+ sEventBroker->post({EV_ADA_READY}, TOPIC_ADA);
+
+ logger.log(calibrator.getReferenceValues());
+ logger.log(ada.getKalmanState());
+ }
+}
+
+/* --- STATES --- */
+/**
+ * \brief Idle state: the ADA waits for a command to start calibration. This
+ * is the initial state.
+ */
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::state_idle(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ TRACE("[ADA] Entering state idle\n");
+ logStatus(ADAState::IDLE);
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[ADA] Exiting state idle\n");
+ break;
+ }
+ case EV_CALIBRATE:
+ {
+ TRACE("[ADA] EV_CALIBRATE \n");
+ this->transition(&ADACtrl::state_calibrating);
+ break;
+ }
+ default:
+ {
+ TRACE("[ADA] state idle: %d event not handled\n", ev.sig);
+ break;
+ }
+ }
+}
+
+/**
+ * \brief Calibrating state: the ADA calibrates the initial Kalman state.
+ *
+ * In this state a call to update() will result in a altitude sample being
+ * added to the average. The exiting transition to the idle state is
+ * triggered at the first sample update after having set the deployment
+ * altitude and having reached the minimum number of calibration samples.
+ */
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::state_calibrating(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ {
+ Lock<FastMutex> l(calibrator_mutex);
+ calibrator.resetBaro();
+ }
+ logStatus(ADAState::CALIBRATING);
+ TRACE("[ADA] Entering state calibrating\n");
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[ADA] Exiting state calibrating\n");
+ break;
+ }
+ case EV_ADA_READY:
+ {
+ this->transition(&ADACtrl::state_ready);
+ break;
+ }
+ case EV_CALIBRATE_ADA:
+ {
+ this->transition(&ADACtrl::state_calibrating);
+ break;
+ }
+ default:
+ {
+ // TRACE("ADA state calibrating: %d event not handled\n",
+ // ev.sig);
+ break;
+ }
+ }
+}
+
+/**
+ * \brief Ready 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.
+ */
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::state_ready(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(ADAState::READY);
+ TRACE("[ADA] Entering state ready\n");
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[ADA] Exiting state ready\n");
+ break;
+ }
+ case EV_LIFTOFF:
+ {
+ this->transition(&ADACtrl::state_shadowMode);
+ break;
+ }
+ case EV_CALIBRATE_ADA:
+ {
+ this->transition(&ADACtrl::state_calibrating);
+ break;
+ }
+ default:
+ {
+ // TRACE("ADA state ready: %d event not handled\n", 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.
+ */
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::state_shadowMode(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ shadow_delayed_event_id =
+ sEventBroker->postDelayed<TIMEOUT_ADA_SHADOW_MODE>(
+ {EV_TIMEOUT_SHADOW_MODE}, TOPIC_ADA);
+ logStatus(ADAState::SHADOW_MODE);
+ TRACE("[ADA] Entering state shadowMode\n");
+ break;
+ }
+ case EV_EXIT:
+ {
+ sEventBroker->removeDelayed(shadow_delayed_event_id);
+ TRACE("[ADA] Exiting state shadowMode\n");
+ break;
+ }
+ case EV_TIMEOUT_SHADOW_MODE:
+ {
+ this->transition(&ADACtrl::state_active);
+ break;
+ }
+ default:
+ {
+ // TRACE("ADA state shadowMode: %d event not handled\n", 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!)
+ */
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::state_active(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(ADAState::ACTIVE);
+ TRACE("[ADA] Entering state active\n");
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[ADA] Exiting state active\n");
+ break;
+ }
+ case EV_ADA_APOGEE_DETECTED:
+ {
+ this->transition(&ADACtrl::state_pressureStabilization);
+ break;
+ }
+ default:
+ {
+ // TRACE("ADA state active: %d event not handled\n", ev.sig);
+ break;
+ }
+ }
+}
+
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::state_pressureStabilization(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ pressure_delayed_event_id =
+ sEventBroker->postDelayed<TIMEOUT_ADA_P_STABILIZATION>(
+ {EV_TIMEOUT_PRESS_STABILIZATION}, TOPIC_ADA);
+ logStatus(ADAState::PRESSURE_STABILIZATION);
+ TRACE("[ADA] Entering state pressureStabilization\n");
+ break;
+ }
+ case EV_EXIT:
+ {
+ sEventBroker->removeDelayed(pressure_delayed_event_id);
+ TRACE("[ADA] Exiting state pressureStabilization\n");
+ break;
+ }
+ case EV_TIMEOUT_PRESS_STABILIZATION:
+ {
+ this->transition(&ADACtrl::state_drogueDescent);
+ break;
+ }
+ default:
+ {
+ // TRACE("ADA state pressureStabilization: %d event not
+ // handled\n", ev.sig);
+ break;
+ }
+ }
+}
+
+/**
+ * \brief First descent phase state: ADA is running and it generates an
+ * event when a set 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 altitude. The exiting transition
+ * to the stop state is triggered by the parachute deployment altitude
+ * reached event (NOT self generated!)
+ */
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::state_drogueDescent(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(ADAState::DROGUE_DESCENT);
+ TRACE("[ADA] Entering state drogueDescent\n");
+ n_samples_deployment_detected = 0;
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[ADA] Exiting state drogueDescent\n");
+ break;
+ }
+ case EV_ADA_DPL_ALT_DETECTED:
+ {
+ status.dpl_altitude_reached = true;
+ logStatus();
+
+ this->transition(&ADACtrl::state_end);
+ break;
+ }
+ default:
+ {
+ // TRACE("ADA state drogueDescent: %d event not handled\n",
+ // 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
+ */
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::state_end(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ TRACE("[ADA] Entering state end\n");
+ logStatus(ADAState::END);
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[ADA] Exiting state end\n");
+ break;
+ }
+ default:
+ {
+ // TRACE("ADA state end: %d event not handled\n", ev.sig);
+ break;
+ }
+ }
+}
+
+/* --- LOGGER --- */
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::logStatus(ADAState state)
+{
+ status.state = state;
+ logStatus();
+}
+
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::logStatus()
+{
+ status.timestamp = TimestampTimer::getTimestamp();
+ logger.log(status);
+
+ StackLogger::getInstance()->updateStack(THID_ADA_FSM);
+}
+
+template <typename Press, typename GPS>
+void ADAController<Press, GPS>::logData(const ADAKalmanState& s,
+ const ADAData& d)
+{
+ logger.log(s);
+ logger.log(d);
+}
} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/ADA/ADAStatus.h b/src/boards/DeathStack/ADA/ADAStatus.h
index 7fc05b37c3f30382759b7d2b4b2a60e34c6e2338..f034c9716b3f224f09710dcefaa40796e3a5db4a 100644
--- a/src/boards/DeathStack/ADA/ADAStatus.h
+++ b/src/boards/DeathStack/ADA/ADAStatus.h
@@ -22,14 +22,17 @@
#pragma once
-#include <configs/ADA_config.h>
+#include <configs/ADAconfig.h>
#include <math/Stats.h>
+
#include <ostream>
namespace DeathStackBoard
{
-// All possible states of the ADA FMM
+using namespace ADAConfigs;
+
+// All possible states of the ADA
enum class ADAState
{
UNDEFINED,
@@ -38,16 +41,16 @@ enum class ADAState
READY,
SHADOW_MODE,
ACTIVE,
- FIRST_DESCENT_PHASE,
- END,
- PRESSURE_STABILIZATION
+ PRESSURE_STABILIZATION,
+ DROGUE_DESCENT,
+ END
};
// Struct to log apogee detection
struct ApogeeDetected
{
ADAState state;
- long long tick;
+ uint64_t tick;
static std::string header() { return "timestamp,state\n"; }
@@ -60,7 +63,7 @@ struct ApogeeDetected
// Struct to log deployment pressure detection
struct DplAltitudeReached
{
- long long tick;
+ uint64_t tick;
static std::string header() { return "tick\n"; }
void print(std::ostream& os) const { os << tick << "\n"; }
@@ -70,7 +73,7 @@ struct DplAltitudeReached
// Also used to keep track of current state
struct ADAControllerStatus
{
- long long timestamp;
+ uint64_t timestamp;
ADAState state = ADAState::UNDEFINED;
bool apogee_reached = false;
bool dpl_altitude_reached = false;
@@ -89,25 +92,21 @@ struct ADAControllerStatus
};
// Struct to log the two Kalman states
-struct KalmanState
+struct ADAKalmanState
{
- long long timestamp;
+ uint64_t timestamp;
float x0;
float x1;
float x2;
- float x0_acc;
- float x1_acc;
- float x2_acc;
static std::string header()
{
- return "timestamp,x0,x1,x2,x0_acc,x1_acc,x2_acc\n";
+ return "timestamp,x0,x1,x2\n";
}
void print(std::ostream& os) const
{
- os << timestamp << "," << x0 << "," << x1 << "," << x2 << "," << x0_acc
- << "," << x1_acc << "," << x2_acc << "\n";
+ os << timestamp << "," << x0 << "," << x1 << "," << x2 << "\n";
}
};
@@ -115,26 +114,21 @@ struct KalmanState
// (state after conversion)
struct ADAData
{
- long long timestamp;
+ uint64_t timestamp;
float msl_altitude;
float dpl_altitude;
bool is_dpl_altitude_agl;
float vert_speed;
- float acc_msl_altitude;
- float acc_vert_speed;
-
static std::string header()
{
- return "timestamp,msl_altitude,dpl_altitude,is_agl,vert_speed,acc_msl_"
- "altitude,acc_vert_speed\n";
+ return "timestamp,msl_altitude,dpl_altitude,is_agl,vert_speed\n";
}
void print(std::ostream& os) const
{
os << timestamp << "," << msl_altitude << "," << dpl_altitude << ","
- << (int)is_dpl_altitude_agl << "," << vert_speed << ","
- << acc_msl_altitude << "," << acc_vert_speed << "\n";
+ << (int)is_dpl_altitude_agl << "," << vert_speed << "\n";
}
};
diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.cpp b/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.cpp
index bfee5bb11ed874431198e592b524aef2ae6d0aa8..f3f77d31eedd821228499d65168e6d571fa2e7c8 100644
--- a/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.cpp
+++ b/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.cpp
@@ -6,7 +6,7 @@
namespace elevationmap
{
-int getElevation(double lat, double lon)
+int getElevation(float lat, float lon)
{
if (lat < SOUTH || lat >= NORTH || lon < WEST || lon >= EAST)
{
diff --git a/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.h b/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.h
index 5d32885fed2b2e649097c4976af76b33333195a0..4438ae440d12f3a2db4a6628258e60653b1b440a 100644
--- a/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.h
+++ b/src/boards/DeathStack/ADA/DeploymentUtils/elevation_map.h
@@ -30,6 +30,6 @@
namespace elevationmap
{
-int getElevation(double lat, double lon);
+int getElevation(float lat, float lon);
} // namespace elevationmap
diff --git a/src/boards/DeathStack/AeroBrakesController/AeroBrakes.scxml b/src/boards/DeathStack/AeroBrakesController/AeroBrakes.scxml
new file mode 100644
index 0000000000000000000000000000000000000000..4eb7408f8e44f84d1bab3bc797c9c0d992b5a8f2
--- /dev/null
+++ b/src/boards/DeathStack/AeroBrakesController/AeroBrakes.scxml
@@ -0,0 +1,41 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<scxml xmlns="http://www.w3.org/2005/07/scxml" initial="initialization" version="1.0">
+ <state id="initialization">
+ <onentry>initServo()</onentry>
+ <onentry>transition(idle)</onentry>
+ </state>
+ <state id="idle">
+ <transition event="FLIGHT_EVENTS.EV_LIFTOFF" target="shadowMode" />
+ <transition event="ABK.EV_WIGGLE_SERVO" target="idle">
+ wiggleServo()
+ </transition>
+ <transition event="ABK.EV_RESET_SERVO" target="idle">
+ resetServo()
+ </transition>
+ <transition event="ABK.EV_TEST_ABK" target="testAirbrakes" />
+ </state>
+ <state id="shadowMode">
+ <onentry>postD(EV_SHADOW_MODE_TIMEOUT)</onentry>
+ <transition event="ABK.EV_SHADOW_MODE_TIMEOUT" target="enabled" />
+ <transition event="ABK.EV_DISABLE_ABK" target="disabled" />
+ </state>
+ <state id="enabled">
+ <onentry>enableAirbrakes()</onentry>
+ <transition event="FLIGHT_EVENTS.EV_APOGEE" target="end" />
+ <transition event="ABK.EV_DISABLE_ABK" target="disabled" />
+ </state>
+ <state id="end">
+ <onentry>setFullyClosed()</onentry>
+ </state>
+ <state id="disabled">
+ <onentry>setFullyClosed()</onentry>
+ </state>
+ <state id="testAirbrakes">
+ <onentry>enableAirbrakes()</onentry>
+ <onentry>incrementallyOpen()</onentry>
+ <onentry>postD(ABK.EV_TEST_TIMEOUT)</onentry>
+ <onexit>incrementallyClose()</onexit>
+ <onexit>disableAirbrakes()</onexit>
+ <transition event="ABK.EV_TEST_TIMEOUT" target="idle" />
+ </state>
+</scxml>
diff --git a/src/boards/DeathStack/AeroBrakesController/AeroBrakesControlAlgorithm.h b/src/boards/DeathStack/AeroBrakesController/AeroBrakesControlAlgorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..2339050e0460840d5c6a083ed70fa40d1035055b
--- /dev/null
+++ b/src/boards/DeathStack/AeroBrakesController/AeroBrakesControlAlgorithm.h
@@ -0,0 +1,434 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Vincenzo Santomarco
+ *
+ * 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 <algorithm>
+#include <type_traits>
+
+#include "../Algorithm.h"
+#include "../ServoInterface.h"
+#include "AeroBrakesData.h"
+#include "Pid.h"
+#include "configs/AeroBrakesConfig.h"
+#include "sensors/Sensor.h"
+#include "trajectories/Trajectory.h"
+#include "TimestampTimer.h"
+
+/*
+ Error and timing benchmark results:
+
+ | float | double
+ ==========================================================
+ Max setpoint error: | 0.000122 | 0.000001
+ Max u error: | 0.001045 | 0.000052
+ Max deltas error: | 0.000000 | 0.000000
+ Max alpha error: | 0 | 0
+ Init time: | 0.179810 ms | 2.037333 ms
+ Avg iter time: | 0.151057 | 1.065261 ms
+ Total computation time: | 25.557320 ms | 181.001143 ms
+
+*/
+
+using namespace DeathStackBoard::AeroBrakesConfigs;
+
+namespace DeathStackBoard
+{
+
+template <class T>
+class AeroBrakesControlAlgorithm : public Algorithm
+{
+private:
+ uint32_t indexMinVal = 0;
+ float alpha = 0;
+ uint64_t ts = 0;
+ Trajectory chosenTrajectory;
+ ServoInterface* actuator;
+ Sensor<T>& sensor;
+ Pid pid;
+ AeroBrakesData data;
+
+ std::is_same<float, decltype(std::declval<T>().z)> checkz;
+ std::is_same<float, decltype(std::declval<T>().vz)> checkvz;
+ std::is_same<float, decltype(std::declval<T>().vMod)> checkvMod;
+ std::is_same<uint16_t, decltype(std::declval<T>().timestamp)>
+ checktimestamp;
+
+public:
+ AeroBrakesControlAlgorithm(Sensor<T>& sensor,
+ ServoInterface* actuator);
+
+ bool init() override { return true; }
+
+ /**
+ * @brief This method starts the algorithm and chooses the trajectory the
+ * rocket will follow for reaching apogee.
+ * */
+ void begin();
+
+ /**
+ * @brief This method looks for nearest point in the current chosen
+ * trajectory and sends to the servoInterface the aerobrakes degree
+ * according to the current rocket speed and the one in the prediction.
+ * */
+ void step() override;
+
+ /**
+ * @brief Computes the alpha degree (in radiants) to be sent to the
+ * servoInterface in order to follow the choosen trajectory. If
+ * firstIteration is true then the nearest trajectory to the current
+ * rocket's coordinates is chosen.
+ *
+ * @param firstIteration The flag indicating wheter the trajectory has
+ * already been choosen or not.
+ *
+ * @returns The alpha in radiants to be sent to the servoInterface
+ * */
+ float computeAlpha(bool firstIteration);
+
+ /**
+ * @brief Chooses the nearest point on the trajectory, considering all the
+ * trajectories. Given the rocket initial input, stores the nearest
+ * trajectory.
+ *
+ * @param z The altitude of the rocket
+ * @param vz The z component of the rocket's speed
+ * */
+ TrajectoryPoint chooseTrajectory(float z, float vz);
+
+ /**
+ * @brief Starting from the second iteration selects the nearest point on
+ * the choosen trajectory. This method does not consider the Euclidean
+ * distance, but only the altitude (z) distance.
+ *
+ * @param z The altitude of the rocket
+ * @param vz The z component of the rocket's speed
+ * */
+ TrajectoryPoint getSetpoint(float z, float vz);
+
+ /**
+ * @brief Update Pid internal input. The Pid output represents represents
+ * the drag force of the wind.
+ *
+ * @param vz The z component of the rocket's speed
+ * @param vMod Modulo of the rocket's speed
+ * @param rho The density of the air
+ * @param setpoint Nearest point to the current rocket input in the chosen
+ * trajectory
+ * */
+ float pidStep(float vz, float vMod, float rho, TrajectoryPoint setpoint);
+
+ /**
+ * @brief Compute the necessary aerobrakes surface to match the
+ * given the drag force from the Pid. The possible deltaS values are
+ * discretized in (DELTA_S_AVAILABLE_MIN, DELTA_S_AVAILABLE_MAX) with a
+ * step of DELTA_S_AVAILABLE_STEP. For every possible deltaS the
+ * correspondig drag force is computed with @see getDrag method and the one
+ * that gives lowest error with respect to Pid output is returned.
+ *
+ * @param z The current altitude of the rocket
+ * @param vz The z component of the rocket's speed
+ * @param vMod Modulo of the rocket's speed
+ * @param rho The density of the air
+ * @param u The current pid value
+ * */
+ float getDeltaS(float z, float vz, float vMod, float rho, float u);
+
+ /**
+ * @brief Maps the exposed aerobrakes surface to servoInterface angle in
+ * radiants.
+ *
+ * @param deltas Total variation of the aerobrakes surface
+ * */
+ float getAlpha(float deltaS);
+
+ /**
+ * @param h The current altitude
+ * @return The density of air according to current altitude.
+ * */
+ float getRho(float h);
+
+ /**
+ * @param v_mod Modulo of the current rocket speed.
+ * @param z The z component of the rocket's speed
+ * @return The speed in Mach unit.
+ * */
+ float getMach(float v_mod, float z);
+
+ /**
+ * @param deltaS The total variation of the aerobrakes surface
+ * @return The radial distance of the current aerobrakes extension.
+ * */
+ float getExtension(float deltaS);
+
+ /**
+ * @param h The current altitude of the rocket
+ * @param s
+ * @param powMach Array containing precomputed powers of the mach
+ * @return The drag force coefficient
+ * */
+ float getDrag(float h, float s, float* powMach);
+
+ /**
+ * @brief Log algorithm data structure
+ *
+ */
+ void logData();
+};
+
+template <class T>
+AeroBrakesControlAlgorithm<T>::AeroBrakesControlAlgorithm(
+ Sensor<T>& sensor, ServoInterface* actuator)
+ : actuator(actuator), sensor(sensor), pid(Kp, Ki)
+{
+}
+
+template <class T>
+void AeroBrakesControlAlgorithm<T>::begin()
+{
+ if (running)
+ {
+ return;
+ }
+
+ running = true;
+
+ ts = (sensor.getLastSample()).timestamp;
+
+ alpha = computeAlpha(true);
+ actuator->set(alpha);
+}
+
+template <class T>
+void AeroBrakesControlAlgorithm<T>::step()
+{
+ if ((sensor.getLastSample()).timestamp > ts)
+ {
+ ts = (sensor.getLastSample()).timestamp;
+ alpha = computeAlpha(false);
+ }
+
+ actuator->set(alpha);
+
+ logData();
+}
+
+template <class T>
+float AeroBrakesControlAlgorithm<T>::computeAlpha(bool firstIteration)
+{
+ T input = sensor.getLastSample();
+ float z = input.z;
+ float vz = input.vz;
+ float vMod = input.vMod;
+ float rho = getRho(z);
+
+ TrajectoryPoint setpoint;
+
+ if (firstIteration)
+ {
+ setpoint = chooseTrajectory(z, vz);
+ }
+ else
+ {
+ setpoint = getSetpoint(z, vz);
+ }
+
+ float u = pidStep(vz, vMod, rho, setpoint);
+ float deltaS = getDeltaS(z, vz, vMod, rho, u);
+ float alpha = getAlpha(deltaS);
+
+ return alpha;
+}
+
+template <class T>
+TrajectoryPoint AeroBrakesControlAlgorithm<T>::chooseTrajectory(float z,
+ float vz)
+{
+ TrajectoryPoint currentPoint(z, vz);
+
+ float bestMin = INFINITY;
+
+ for (unsigned trajectoryIndex = 0; trajectoryIndex < TOT_TRAJECTORIES;
+ trajectoryIndex++)
+ {
+
+ Trajectory trajectory = Trajectory(trajectoryIndex);
+
+ for (uint32_t pointIndex = 0; pointIndex < LOOKS; pointIndex++)
+ {
+ TrajectoryPoint ref = trajectory.get(pointIndex);
+ float distancesFromCurrentinput =
+ TrajectoryPoint::distance(ref, currentPoint);
+
+ if (distancesFromCurrentinput < bestMin)
+ {
+ bestMin = distancesFromCurrentinput;
+ indexMinVal = pointIndex;
+ chosenTrajectory = trajectory;
+ }
+ }
+ }
+
+ TrajectoryPoint setpoint = chosenTrajectory.get(indexMinVal);
+
+ return setpoint;
+}
+
+template <class T>
+TrajectoryPoint AeroBrakesControlAlgorithm<T>::getSetpoint(float z, float vz)
+{
+ TrajectoryPoint currentPoint(z, vz);
+ float minDistance = INFINITY;
+
+ uint32_t start = std::max(indexMinVal + START_INDEX_OFFSET, (uint32_t)0);
+ uint32_t end = chosenTrajectory.length();
+
+ for (uint32_t pointIndex = start; pointIndex < end; pointIndex++)
+ {
+ TrajectoryPoint ref = chosenTrajectory.get(pointIndex);
+ float DistanceFromCurrentinput =
+ TrajectoryPoint::zDistance(ref, currentPoint);
+ if (DistanceFromCurrentinput < minDistance)
+ {
+ minDistance = DistanceFromCurrentinput;
+ indexMinVal = pointIndex;
+ }
+ }
+
+ TrajectoryPoint setpoint = chosenTrajectory.get(indexMinVal);
+
+ return setpoint;
+}
+
+template <class T>
+float AeroBrakesControlAlgorithm<T>::pidStep(float vz, float vMod, float rho,
+ TrajectoryPoint setpoint)
+{
+ float umin = 0;
+ // The *1 factor replaces the cd variable. This variable will be later
+ // calculated in the getDeltaS method.
+ float umax = 0.5 * rho * S0 * 1 * vz * vMod;
+ float error = (vz - setpoint.getVz());
+
+ float u = pid.step(umin, umax, error);
+
+ return u;
+}
+
+template <class T>
+float AeroBrakesControlAlgorithm<T>::getDeltaS(float z, float vz, float vMod,
+ float rho, float u)
+{
+ float deltaS = 0;
+ float bestValue = INFINITY;
+
+ float mach = getMach(vMod, z);
+ // Precomputes all the powers of mach (from 0 to 6) in order to reuse it
+ // at avery iteration of the deltaSAvailable loop.
+ // See getDrag method.
+ float powMach[7] = {1,
+ mach,
+ powf(mach, 2),
+ powf(mach, 3),
+ powf(mach, 4),
+ powf(mach, 5),
+ powf(mach, 6)};
+
+ for (float deltaSAvailable = DELTA_S_AVAILABLE_MIN;
+ deltaSAvailable < DELTA_S_AVAILABLE_MAX + DELTA_S_AVAILABLE_STEP;
+ deltaSAvailable += DELTA_S_AVAILABLE_STEP)
+ {
+ float cdAvaliable = getDrag(z, deltaSAvailable, powMach);
+ float temp = abs(u - 0.5 * rho * S0 * cdAvaliable * vz * vMod);
+ if (temp < bestValue)
+ {
+ bestValue = temp;
+ deltaS = deltaSAvailable;
+ }
+ }
+
+ return deltaS;
+}
+
+template <class T>
+float AeroBrakesControlAlgorithm<T>::getAlpha(float deltaS)
+{
+ float alphaRadiants =
+ (-B_DELTAS + sqrt(powf(B_DELTAS, 2) + 4 * A_DELTAS * deltaS)) /
+ (2 * A_DELTAS);
+ float alphaDegrees = alphaRadiants * 180.0f / PI;
+
+ return alphaDegrees;
+}
+
+template <class T>
+float AeroBrakesControlAlgorithm<T>::getRho(float h)
+{
+ return RHO * expf(-h / Hn);
+}
+
+template <class T>
+float AeroBrakesControlAlgorithm<T>::getMach(float v_mod, float z)
+{
+ float c = Co + ALPHA * z;
+ return v_mod / c;
+}
+
+template <class T>
+float AeroBrakesControlAlgorithm<T>::getExtension(float deltaS)
+{
+ return (-B + sqrtf(powf(B, 2) + 4 * A * deltaS)) / (2 * A);
+}
+
+template <class T>
+float AeroBrakesControlAlgorithm<T>::getDrag(float h, float s, float* powMach)
+{
+ float x = getExtension(s);
+
+ return coeffs.n000 + coeffs.n100 * powMach[1] + coeffs.n200 * powMach[2] +
+ coeffs.n300 * powMach[3] + coeffs.n400 * powMach[4] +
+ coeffs.n500 * powMach[5] + coeffs.n600 * powMach[6] +
+ coeffs.n010 * x + coeffs.n020 * powf(x, 2) +
+ coeffs.n110 * x * powMach[1] +
+ coeffs.n120 * powf(x, 2) * powMach[1] +
+ coeffs.n210 * x * powMach[2] +
+ coeffs.n220 * powf(x, 2) * powMach[2] +
+ coeffs.n310 * x * powMach[3] +
+ coeffs.n320 * powf(x, 2) * powMach[3] +
+ coeffs.n410 * x * powMach[4] +
+ coeffs.n420 * powf(x, 2) * powMach[4] +
+ coeffs.n510 * x * powMach[5] +
+ coeffs.n520 * powf(x, 2) * powMach[5] + coeffs.n001 * h;
+}
+
+template <class T>
+void AeroBrakesControlAlgorithm<T>::logData()
+{
+ data.timestamp = TimestampTimer::getTimestamp();
+ data.running = running;
+ data.servo_position = actuator->getCurrentPosition();
+
+ // logger.log(data);
+}
+
+} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/AeroBrakesController/AeroBrakesController.h b/src/boards/DeathStack/AeroBrakesController/AeroBrakesController.h
new file mode 100644
index 0000000000000000000000000000000000000000..53d7b05668060baf9bd0070c43cb5ad5c72e9111
--- /dev/null
+++ b/src/boards/DeathStack/AeroBrakesController/AeroBrakesController.h
@@ -0,0 +1,396 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio
+ *
+ * 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 <miosix.h>
+
+#include "AeroBrakesController/AeroBrakesControlAlgorithm.h"
+#include "AeroBrakesController/AeroBrakesServo.h"
+#include "AeroBrakesData.h"
+#include "TimestampTimer.h"
+#include "configs/AeroBrakesConfig.h"
+#include "events/EventBroker.h"
+#include "events/Events.h"
+#include "events/FSM.h"
+
+namespace DeathStackBoard
+{
+
+/**
+ * @brief AeroBrakes state machine
+ */
+template <class T>
+class AeroBrakesController : public FSM<AeroBrakesController<T>>
+{
+public:
+ AeroBrakesController(Sensor<T>& sensor,
+ ServoInterface* servo = new AeroBrakesServo());
+ ~AeroBrakesController();
+
+ // Aerobrakes FSM states
+ void state_initialization(const Event& ev);
+ void state_idle(const Event& ev);
+ void state_shadowMode(const Event& ev);
+ void state_enabled(const Event& ev);
+ void state_end(const Event& ev);
+ void state_disabled(const Event& ev);
+ void state_testAerobrakes(const Event& ev);
+
+ /**
+ * @brief Update the aerobrakes control algorithm
+ */
+ void update();
+
+private:
+ AeroBrakesControllerStatus status;
+ ServoInterface* servo;
+ AeroBrakesControlAlgorithm<T> algorithm;
+
+ uint16_t ev_shadow_mode_timeout_id;
+
+ /**
+ * @brief Incrementally opens the servo with steps of 10°
+ */
+ void incrementallyOpen();
+
+ /**
+ * @brief Incrementally closes the servo with steps of 10°
+ */
+ void incrementallyClose();
+
+ void logStatus(AeroBrakesControllerState state);
+};
+
+template <class T>
+AeroBrakesController<T>::AeroBrakesController(Sensor<T>& sensor,
+ ServoInterface* servo)
+ : FSM<AeroBrakesController<T>>(
+ &AeroBrakesController<T>::state_initialization),
+ servo(servo), algorithm(sensor, servo)
+{
+ memset(&status, 0, sizeof(AeroBrakesControllerStatus));
+ sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS);
+ sEventBroker->subscribe(this, TOPIC_ABK);
+}
+
+template <class T>
+AeroBrakesController<T>::~AeroBrakesController()
+{
+ sEventBroker->unsubscribe(this);
+}
+
+template <class T>
+void AeroBrakesController<T>::logStatus(AeroBrakesControllerState state)
+{
+ status.timestamp = TimestampTimer::getTimestamp();
+ status.state = state;
+
+ // logger.log(status);
+
+ // StackLogger::getInstance()->updateStack(THID_ABK_FSM);
+}
+
+template <class T>
+void AeroBrakesController<T>::update()
+{
+ algorithm.update();
+}
+
+template <class T>
+void AeroBrakesController<T>::state_initialization(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ servo->enable();
+ servo->reset();
+
+ this->transition(&AeroBrakesController<T>::state_idle);
+ break;
+ }
+ case EV_EXIT:
+ {
+ break;
+ }
+
+ default:
+ {
+ break;
+ }
+ }
+}
+
+template <class T>
+void AeroBrakesController<T>::state_idle(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ logStatus(AeroBrakesControllerState::IDLE);
+
+ TRACE("[AeroBrakes] entering state idle\n");
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[AeroBrakes] exiting state idle\n");
+ break;
+ }
+ case EV_LIFTOFF:
+ {
+ this->transition(&AeroBrakesController<T>::state_shadowMode);
+ break;
+ }
+ case EV_WIGGLE_SERVO:
+ {
+ servo->selfTest();
+ break;
+ }
+ case EV_RESET_SERVO:
+ {
+ servo->reset();
+ break;
+ }
+ case EV_TEST_ABK:
+ {
+ this->transition(&AeroBrakesController<T>::state_testAerobrakes);
+ break;
+ }
+ default:
+ {
+ break;
+ }
+ }
+}
+
+template <class T>
+void AeroBrakesController<T>::state_shadowMode(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ ev_shadow_mode_timeout_id =
+ sEventBroker->postDelayed<SHADOW_MODE_DURATION>(
+ Event{EV_SHADOW_MODE_TIMEOUT}, TOPIC_FLIGHT_EVENTS);
+
+ logStatus(AeroBrakesControllerState::SHADOW_MODE);
+
+ TRACE("[AeroBrakes] entering state shadow_mode\n");
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[AeroBrakes] exiting state shadow_mode\n");
+ break;
+ }
+ case EV_SHADOW_MODE_TIMEOUT:
+ {
+ this->transition(&AeroBrakesController<T>::state_enabled);
+ break;
+ }
+ case EV_DISABLE_ABK:
+ {
+ this->transition(&AeroBrakesController<T>::state_disabled);
+ break;
+ }
+ default:
+ {
+ break;
+ }
+ }
+}
+
+template <class T>
+void AeroBrakesController<T>::state_enabled(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ algorithm.begin();
+
+ logStatus(AeroBrakesControllerState::ENABLED);
+
+ TRACE("[AeroBrakes] entering state enabled\n");
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[AeroBrakes] exiting state enabled\n");
+ break;
+ }
+ case EV_APOGEE:
+ {
+ this->transition(&AeroBrakesController<T>::state_end);
+ break;
+ }
+ case EV_DISABLE_ABK:
+ {
+ this->transition(&AeroBrakesController<T>::state_disabled);
+ break;
+ }
+ default:
+ {
+ break;
+ }
+ }
+}
+
+template <class T>
+void AeroBrakesController<T>::state_end(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ algorithm.end();
+ servo->reset();
+
+ logStatus(AeroBrakesControllerState::END);
+
+ TRACE("[AeroBrakes] entering state end\n");
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[AeroBrakes] exiting state end\n");
+ break;
+ }
+
+ default:
+ {
+ break;
+ }
+ }
+}
+
+template <class T>
+void AeroBrakesController<T>::state_disabled(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ algorithm.end();
+ servo->reset();
+
+ logStatus(AeroBrakesControllerState::DISABLED);
+
+ TRACE("[AeroBrakes] entering state disabled\n");
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[AeroBrakes] exiting state disabled\n");
+ break;
+ }
+
+ default:
+ {
+ break;
+ }
+ }
+}
+
+template <class T>
+void AeroBrakesController<T>::state_testAerobrakes(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
+ {
+ TRACE("[AeroBrakes] entering state test_aerobrakes\n");
+
+ incrementallyOpen();
+ miosix::Thread::sleep(1000);
+ incrementallyClose();
+ miosix::Thread::sleep(1000);
+ servo->reset();
+
+ logStatus(AeroBrakesControllerState::TEST_AEROBRAKES);
+
+ sEventBroker->post(Event{EV_TEST_TIMEOUT}, TOPIC_ABK);
+ break;
+ }
+ case EV_EXIT:
+ {
+ TRACE("[AeroBrakes] exiting state test_aerobrakes\n");
+ break;
+ }
+ case EV_TEST_TIMEOUT:
+ {
+ this->transition(&AeroBrakesController<T>::state_idle);
+ break;
+ }
+ default:
+ {
+ break;
+ }
+ }
+}
+
+template <class T>
+void AeroBrakesController<T>::incrementallyOpen()
+{
+ // Equal steps of about 5°
+ const int STEPS_NUM = (servo->MAX_POS - servo->MIN_POS) / 10.0f;
+ const float INCREMENT_STEP = (servo->MAX_POS - servo->MIN_POS) / STEPS_NUM;
+
+ float currentStep = servo->MIN_POS;
+
+ for (auto i = 0; i < STEPS_NUM; i++)
+ {
+ TRACE("Servo position : %.2f\n", currentStep);
+ servo->set(currentStep);
+ currentStep += INCREMENT_STEP;
+ miosix::Thread::sleep(1000);
+ }
+
+ servo->set(servo->MAX_POS);
+}
+
+template <class T>
+void AeroBrakesController<T>::incrementallyClose()
+{
+ // Equal steps of about 5°
+ const int STEPS_NUM = (servo->MAX_POS - servo->MIN_POS) / 10.0f;
+ const float INCREMENT_STEP = (servo->MAX_POS - servo->MIN_POS) / STEPS_NUM;
+
+ float currentStep = servo->MAX_POS;
+
+ for (auto i = 0; i < STEPS_NUM; i++)
+ {
+ TRACE("Servo position : %.2f\n", currentStep);
+ servo->set(currentStep);
+ currentStep -= INCREMENT_STEP;
+ miosix::Thread::sleep(1000);
+ }
+
+ servo->set(servo->MIN_POS);
+}
+
+} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/AeroBrakesController/AeroBrakesData.h b/src/boards/DeathStack/AeroBrakesController/AeroBrakesData.h
new file mode 100644
index 0000000000000000000000000000000000000000..b96ff70f7c596e3341ba8e9e2d2ce7566959d0bf
--- /dev/null
+++ b/src/boards/DeathStack/AeroBrakesController/AeroBrakesData.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio
+ *
+ * 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 <stdint.h>
+
+#include <iostream>
+#include <string>
+
+namespace DeathStackBoard
+{
+
+/**
+ * Enum defining the possibile FSM states.
+ */
+enum AeroBrakesControllerState : uint8_t
+{
+ IDLE = 0,
+ SHADOW_MODE,
+ ENABLED,
+ END,
+ DISABLED,
+ TEST_AEROBRAKES,
+};
+
+/**
+ * Structure defining the overall controller status.
+ */
+struct AeroBrakesControllerStatus
+{
+ long long timestamp;
+ AeroBrakesControllerState state;
+
+ static std::string header() { return "timestamp,state\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << (int)state << "\n";
+ }
+};
+
+/**
+ * Structure defining the output of the control algorithm.
+ */
+struct AeroBrakesData
+{
+ long long timestamp;
+ float running;
+ float servo_position;
+
+ static std::string header() { return "timestamp,running,servo_position\n"; }
+
+ void print(std::ostream& os) const
+ {
+ os << timestamp << "," << (int)running << "," << servo_position << "\n";
+ }
+};
+
+} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/AeroBrakesController/AeroBrakesServo.h b/src/boards/DeathStack/AeroBrakesController/AeroBrakesServo.h
new file mode 100644
index 0000000000000000000000000000000000000000..d6a4475c84562d545e31fec606010ca115f960d9
--- /dev/null
+++ b/src/boards/DeathStack/AeroBrakesController/AeroBrakesServo.h
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Vincenzo Santomarco
+ *
+ * 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 "../ServoInterface.h"
+#include "../configs/AeroBrakesConfig.h"
+#include "drivers/servo/servo.h"
+#include "miosix.h"
+
+using namespace DeathStackBoard::AeroBrakesConfigs;
+
+namespace DeathStackBoard
+{
+class AeroBrakesServo : public ServoInterface
+{
+public:
+ AeroBrakesServo() : ServoInterface(SERVO_MIN_POS, SERVO_MAX_POS) {}
+ AeroBrakesServo(float minPosition, float maxPosition)
+ : ServoInterface(minPosition, maxPosition)
+ {
+ }
+ AeroBrakesServo(float minPosition, float maxPosition, float resetPosition)
+ : ServoInterface(minPosition, maxPosition, resetPosition)
+ {
+ }
+
+ void enable() override
+ {
+ servo.enable(SERVO_PWM_CH);
+ servo.start();
+ }
+
+ void disable() override
+ {
+ servo.stop();
+ servo.disable(SERVO_PWM_CH);
+ }
+
+ /**
+ * @brief Perform wiggle around the middle point.
+ */
+ void selfTest() override
+ {
+ float base = (MAX_POS + RESET_POS) / 2;
+ float maxpos = base + SERVO_WIGGLE_AMPLITUDE / 2;
+ float minpos = base - SERVO_WIGGLE_AMPLITUDE / 2;
+
+ set(base);
+
+ for (int i = 0; i < 3; i++)
+ {
+ miosix::Thread::sleep(UPDATE_TIME + 100);
+ set(maxpos);
+ miosix::Thread::sleep(UPDATE_TIME + 100);
+ set(minpos);
+ }
+
+ miosix::Thread::sleep(UPDATE_TIME);
+ }
+
+private:
+ Servo servo{SERVO_TIMER};
+
+protected:
+ /**
+ * @brief Set servo position.
+ *
+ * @param angle servo position (in degrees)
+ */
+ void setPosition(float angle) override
+ {
+ currentPosition = angle;
+ // map position to [0;1] interval for the servo driver
+ servo.setPosition(AeroBrakesConfigs::SERVO_PWM_CH, angle / 180.0f);
+ }
+
+ float preprocessPosition(float angle) override
+ {
+ angle = ServoInterface::preprocessPosition(angle);
+
+ float rate = (angle - currentPosition) / UPDATE_TIME;
+
+ if (rate > SERVO_MAX_RATE)
+ {
+ angle = UPDATE_TIME * SERVO_MAX_RATE + currentPosition;
+ }
+ else if (rate < SERVO_MIN_RATE)
+ {
+ angle = UPDATE_TIME * SERVO_MIN_RATE + currentPosition;
+ }
+
+ return angle;
+ }
+};
+} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/scripts/event_header_generator/Topics.h.template b/src/boards/DeathStack/AeroBrakesController/Coeffs.h
similarity index 58%
rename from scripts/event_header_generator/Topics.h.template
rename to src/boards/DeathStack/AeroBrakesController/Coeffs.h
index 87986edbad2dfd6f72a2377389d43da214e85816..53b95075b53505504c40578c915641bd906ba85f 100644
--- a/scripts/event_header_generator/Topics.h.template
+++ b/src/boards/DeathStack/AeroBrakesController/Coeffs.h
@@ -1,5 +1,6 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Vincenzo Santomarco
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -20,42 +21,33 @@
* THE SOFTWARE.
*/
-/*
- ******************************************************************************
- * THIS FILE IS AUTOGENERATED. DO NOT EDIT. *
- ******************************************************************************
- */
-
-// Generated from: {sheet_link}
-// Autogen date: {date}
-
#pragma once
-#include <stdint.h>
-#include <string>
-#include <vector>
-
-using std::string;
-
namespace DeathStackBoard
-{{
-/**
- * Definition of various event topics to use in the EventBroker
- */
-enum Topics : uint8_t
-{{
-{enum_data}
-}};
-
-const std::vector<uint8_t> TOPIC_LIST {{{topic_list}}};
-
-/**
- * @brief Returns the name of the provided event
- *
- * @param event
- * @return string
- */
-string getTopicString(uint8_t topic);
-
-}} // namespace DeathStackBoard
-
+{
+typedef struct
+{
+ float n000 = 0.496878;
+ float n100 = -1.405038;
+ float n200 = 6.502618;
+ float n300 = -18.314261;
+ float n400 = 30.152448;
+ float n500 = -26.715700;
+ float n600 = 9.711730;
+ float n010 = 8.486901;
+ float n020 = 141.050398;
+ float n110 = 1.233043;
+ float n120 = -152.610100;
+ float n210 = 81.980768;
+ float n220 = 1072.170007;
+ float n310 = -309.620754;
+ float n320 = -3618.989164;
+ float n410 = 455.524477;
+ float n420 = 5190.202262;
+ float n510 = -212.545170;
+ float n520 = -2402.939515;
+ float n001 = 0.000003;
+} coeffs_t;
+
+const coeffs_t coeffs;
+} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/AeroBrakesController/Pid.h b/src/boards/DeathStack/AeroBrakesController/Pid.h
new file mode 100644
index 0000000000000000000000000000000000000000..2d85e2a3d0a990a47e25fff3784f50cd34d91a20
--- /dev/null
+++ b/src/boards/DeathStack/AeroBrakesController/Pid.h
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Vincenzo Santomarco
+ *
+ * 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 "Coeffs.h"
+
+namespace DeathStackBoard
+{
+/**
+ * @brief Pid proportional and integral controller with saturation
+ * */
+class Pid
+{
+private:
+ const float Kp;
+ const float Ki;
+
+ float i = 0;
+ bool saturation = false;
+
+public:
+ Pid(float Kp, float Ki) : Kp(Kp), Ki(Ki) {}
+ ~Pid() {}
+
+ /**
+ * @brief Update the Pid internal state
+ * */
+ float step(float umin, float umax, float error)
+ {
+
+ float p = Kp * error;
+
+ if (!saturation)
+ {
+ i = i + Ki * error;
+ }
+
+ float u = p + i;
+
+ if (u < umin)
+ {
+ u = umin;
+ saturation = true;
+ }
+ else if (u > umax)
+ {
+ u = umax;
+ saturation = true;
+ }
+ else
+ {
+ saturation = false;
+ }
+
+ return u;
+ }
+};
+
+} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/AeroBrakesController/readme.md b/src/boards/DeathStack/AeroBrakesController/readme.md
new file mode 100644
index 0000000000000000000000000000000000000000..ef63c80a1a898154e6d6b7e618b677c581064c7f
--- /dev/null
+++ b/src/boards/DeathStack/AeroBrakesController/readme.md
@@ -0,0 +1,12 @@
+```mermaid
+stateDiagram-v2
+ [*] --> initialization
+ initialization --> idle
+ idle --> shadow_mode : EV_LIFTOFF
+ idle --> test_airbrakes : EV_TEST_ABK
+ shadow_mode --> enabled : EV_SHADOW_MODE_TIMEOUT
+ shadow_mode --> disabled : EV_DISABLE_ABK
+ enabled --> end : EV_APOGEE
+ enabled --> disabled : EV_DISABLE_ABK
+ test_airbrakes --> idle : EV_TEST_TIMEOUT
+```
\ No newline at end of file
diff --git a/src/boards/DeathStack/AeroBrakesController/trajectories/Trajectories_data.h b/src/boards/DeathStack/AeroBrakesController/trajectories/Trajectories_data.h
new file mode 100644
index 0000000000000000000000000000000000000000..055934ac9d0ca2669b891fd0ca05169dbf4c787d
--- /dev/null
+++ b/src/boards/DeathStack/AeroBrakesController/trajectories/Trajectories_data.h
@@ -0,0 +1,1727 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Vincenzo Santomarco
+ *
+ * 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
+
+namespace DeathStackBoard
+{
+
+static const unsigned int TOT_TRAJECTORIES = 9;
+static const unsigned int TRAJECTORY_MAX_LENGTH = 500;
+
+struct point_t
+{
+ float z;
+ float vz;
+};
+
+struct trajectory_t
+{
+ unsigned int length;
+ point_t data[TRAJECTORY_MAX_LENGTH];
+};
+
+const trajectory_t TRAJECTORIES_DATA[TOT_TRAJECTORIES] = {
+ {404,
+ {
+ {822.834116, 250.000000}, {822.834116, 250.000000},
+ {825.748986, 249.760711}, {838.211504, 248.739999},
+ {850.623108, 247.724175}, {862.984042, 246.713176},
+ {875.294545, 245.706939}, {887.554854, 244.705404},
+ {899.765202, 243.708514}, {911.925820, 242.716212},
+ {924.036936, 241.728443}, {936.098776, 240.745154},
+ {948.111562, 239.766291}, {960.075515, 238.791806},
+ {971.990851, 237.821647}, {983.857786, 236.855768},
+ {995.676534, 235.894120}, {1007.447303, 234.936658},
+ {1019.170303, 233.983337}, {1030.845739, 233.034114},
+ {1042.473816, 232.088944}, {1054.054734, 231.147786},
+ {1065.588693, 230.210598}, {1077.075892, 229.277341},
+ {1088.516525, 228.347974}, {1099.910786, 227.422460},
+ {1111.258866, 226.500758}, {1122.560956, 225.582833},
+ {1133.817243, 224.668648}, {1145.027913, 223.758166},
+ {1156.193151, 222.851351}, {1167.313139, 221.948170},
+ {1178.388058, 221.048587}, {1189.418087, 220.152568},
+ {1200.403403, 219.260081}, {1211.344183, 218.371092},
+ {1222.240599, 217.485570}, {1233.092825, 216.603481},
+ {1243.901032, 215.724796}, {1254.665389, 214.849483},
+ {1265.386064, 213.977511}, {1276.063223, 213.108851},
+ {1286.697031, 212.243472}, {1297.287652, 211.381346},
+ {1307.835246, 210.522443}, {1318.339976, 209.666734},
+ {1328.801999, 208.814192}, {1339.221474, 207.964789},
+ {1349.598556, 207.118496}, {1359.933400, 206.275288},
+ {1370.226161, 205.435136}, {1380.476990, 204.598014},
+ {1390.686037, 203.763896}, {1400.853454, 202.932756},
+ {1410.979387, 202.104569}, {1421.063984, 201.279308},
+ {1431.107390, 200.456948}, {1441.109750, 199.637465},
+ {1451.071208, 198.820835}, {1460.991905, 198.007031},
+ {1470.871981, 197.196032}, {1480.711577, 196.387812},
+ {1490.510831, 195.582347}, {1500.269880, 194.779616},
+ {1509.988861, 193.979594}, {1519.667907, 193.182259},
+ {1529.307153, 192.387588}, {1538.906732, 191.595558},
+ {1548.466774, 190.806148}, {1557.987411, 190.019334},
+ {1567.468772, 189.235097}, {1576.910985, 188.453413},
+ {1586.314177, 187.674262}, {1595.678474, 186.897622},
+ {1605.004001, 186.123472}, {1614.290883, 185.351793},
+ {1623.539242, 184.582562}, {1632.749200, 183.815761},
+ {1641.920878, 183.051368}, {1651.054396, 182.289363},
+ {1660.149874, 181.529728}, {1669.207428, 180.772442},
+ {1678.227176, 180.017486}, {1687.209234, 179.264841},
+ {1696.153717, 178.514488}, {1705.060740, 177.766407},
+ {1713.930415, 177.020581}, {1722.762854, 176.276990},
+ {1731.558169, 175.535617}, {1740.316470, 174.796443},
+ {1749.037868, 174.059450}, {1757.722470, 173.324621},
+ {1766.370384, 172.591937}, {1774.981716, 171.861382},
+ {1783.556574, 171.132938}, {1792.095063, 170.406588},
+ {1800.597285, 169.682314}, {1809.063346, 168.960101},
+ {1817.493346, 168.239931}, {1825.887389, 167.521788},
+ {1834.245575, 166.805655}, {1842.568005, 166.091516},
+ {1850.854776, 165.379356}, {1859.105989, 164.669158},
+ {1867.321741, 163.960906}, {1875.502128, 163.254585},
+ {1883.647247, 162.550180}, {1891.757194, 161.847674},
+ {1899.832062, 161.147054}, {1907.871946, 160.448303},
+ {1915.876939, 159.751406}, {1923.847132, 159.056350},
+ {1931.782619, 158.363120}, {1939.683490, 157.671700},
+ {1947.549834, 156.982076}, {1955.381742, 156.294235},
+ {1963.179302, 155.608162}, {1970.942602, 154.923843},
+ {1978.671730, 154.241265}, {1986.366772, 153.560412},
+ {1994.027814, 152.881273}, {2001.654941, 152.203833},
+ {2009.248239, 151.528079}, {2016.807791, 150.853998},
+ {2024.333681, 150.181577}, {2031.825990, 149.510802},
+ {2039.284802, 148.841661}, {2046.710197, 148.174141},
+ {2054.102256, 147.508229}, {2061.461059, 146.843913},
+ {2068.786687, 146.181181}, {2076.079217, 145.520019},
+ {2083.338728, 144.860416}, {2090.565297, 144.202360},
+ {2097.759002, 143.545838}, {2104.919919, 142.890839},
+ {2112.048124, 142.237351}, {2119.143691, 141.585362},
+ {2126.206697, 140.934861}, {2133.237215, 140.285836},
+ {2140.235317, 139.638276}, {2147.201078, 138.992170},
+ {2154.134570, 138.347506}, {2161.035865, 137.704273},
+ {2167.905033, 137.062460}, {2174.742146, 136.422056},
+ {2181.547274, 135.783051}, {2188.320486, 135.145434},
+ {2195.061852, 134.509195}, {2201.771439, 133.874321},
+ {2208.449318, 133.240804}, {2215.095554, 132.608633},
+ {2221.710214, 131.977797}, {2228.293366, 131.348287},
+ {2234.845076, 130.720092}, {2241.365408, 130.093203},
+ {2247.854429, 129.467608}, {2254.312201, 128.843300},
+ {2260.738790, 128.220267}, {2267.134260, 127.598500},
+ {2273.498672, 126.977989}, {2279.832090, 126.358726},
+ {2286.134575, 125.740700}, {2292.406190, 125.123902},
+ {2298.646996, 124.508322}, {2304.857053, 123.893953},
+ {2311.036421, 123.280784}, {2317.185161, 122.668806},
+ {2323.303331, 122.058010}, {2329.390991, 121.448388},
+ {2335.448199, 120.839930}, {2341.475013, 120.232629},
+ {2347.471491, 119.626474}, {2353.437689, 119.021458},
+ {2359.373665, 118.417571}, {2365.279474, 117.814806},
+ {2371.155173, 117.213153}, {2377.000817, 116.612605},
+ {2382.816461, 116.013153}, {2388.602160, 115.414789},
+ {2394.357967, 114.817504}, {2400.083937, 114.221291},
+ {2405.780123, 113.626142}, {2411.446577, 113.032047},
+ {2417.083354, 112.439000}, {2422.690503, 111.846993},
+ {2428.268079, 111.256018}, {2433.816131, 110.666066},
+ {2439.334711, 110.077130}, {2444.823869, 109.489203},
+ {2450.283656, 108.902277}, {2455.714122, 108.316344},
+ {2461.115315, 107.731397}, {2466.487286, 107.147428},
+ {2471.830082, 106.564431}, {2477.143753, 105.982397},
+ {2482.428346, 105.401319}, {2487.683909, 104.821190},
+ {2492.910488, 104.242003}, {2498.108132, 103.663751},
+ {2503.276887, 103.086426}, {2508.416798, 102.510023},
+ {2513.527912, 101.934532}, {2518.610274, 101.359948},
+ {2523.663929, 100.786264}, {2528.688923, 100.213473},
+ {2533.685299, 99.641568}, {2538.653101, 99.070542},
+ {2543.592375, 98.500389}, {2548.503162, 97.931102},
+ {2553.385506, 97.362673}, {2558.239451, 96.795098},
+ {2563.065037, 96.228368}, {2567.862308, 95.662479},
+ {2572.631306, 95.097422}, {2577.372071, 94.533192},
+ {2582.084646, 93.969782}, {2586.769070, 93.407186},
+ {2591.425384, 92.845398}, {2596.053630, 92.284412},
+ {2600.653845, 91.724220}, {2605.226071, 91.164818},
+ {2609.770347, 90.606198}, {2614.286711, 90.048355},
+ {2618.775202, 89.491283}, {2623.235858, 88.934976},
+ {2627.668718, 88.379427}, {2632.073820, 87.824631},
+ {2636.451200, 87.270581}, {2640.800896, 86.717273},
+ {2645.122946, 86.164699}, {2649.417384, 85.612855},
+ {2653.684249, 85.061734}, {2657.923576, 84.511331},
+ {2662.135400, 83.961639}, {2666.319757, 83.412654},
+ {2670.476683, 82.864370}, {2674.606212, 82.316780},
+ {2678.708378, 81.769880}, {2682.783217, 81.223664},
+ {2686.830762, 80.678126}, {2690.851046, 80.133261},
+ {2694.844104, 79.589063}, {2698.809969, 79.045527},
+ {2702.748673, 78.502648}, {2706.660250, 77.960420},
+ {2710.544732, 77.418838}, {2714.402150, 76.877896},
+ {2718.232537, 76.337589}, {2722.035925, 75.797913},
+ {2725.812344, 75.258861}, {2729.561826, 74.720429},
+ {2733.284402, 74.182611}, {2736.980103, 73.645403},
+ {2740.648958, 73.108799}, {2744.290997, 72.572793},
+ {2747.906252, 72.037382}, {2751.494750, 71.502560},
+ {2755.056522, 70.968322}, {2758.591597, 70.434663},
+ {2762.100003, 69.901578}, {2765.581769, 69.369062},
+ {2769.036923, 68.837111}, {2772.465494, 68.305719},
+ {2775.867509, 67.774881}, {2779.242996, 67.244594},
+ {2782.591982, 66.714851}, {2785.914495, 66.185648},
+ {2789.210560, 65.656981}, {2792.480206, 65.128845},
+ {2795.723458, 64.601234}, {2798.940342, 64.074145},
+ {2802.130885, 63.547573}, {2805.295112, 63.021513},
+ {2808.433049, 62.495960}, {2811.544721, 61.970911},
+ {2814.630153, 61.446359}, {2817.689369, 60.922302},
+ {2820.722395, 60.398734}, {2823.729255, 59.875651},
+ {2826.709972, 59.353048}, {2829.664572, 58.830921},
+ {2832.593076, 58.309266}, {2835.495510, 57.788078},
+ {2838.371896, 57.267353}, {2841.222257, 56.747087},
+ {2844.046616, 56.227274}, {2846.844995, 55.707912},
+ {2849.617418, 55.188995}, {2852.363906, 54.670519},
+ {2855.084481, 54.152480}, {2857.779165, 53.634875},
+ {2860.447979, 53.117698}, {2863.090945, 52.600945},
+ {2865.708084, 52.084613}, {2868.299417, 51.568697},
+ {2870.864964, 51.053193}, {2873.404746, 50.538098},
+ {2875.918784, 50.023406}, {2878.407097, 49.509114},
+ {2880.869705, 48.995219}, {2883.306629, 48.481715},
+ {2885.717886, 47.968599}, {2888.103498, 47.455867},
+ {2890.463483, 46.943515}, {2892.797859, 46.431540},
+ {2895.106646, 45.919936}, {2897.389862, 45.408701},
+ {2899.647525, 44.897831}, {2901.879654, 44.387321},
+ {2904.086266, 43.877168}, {2906.267380, 43.367368},
+ {2908.423012, 42.857917}, {2910.553180, 42.348812},
+ {2912.657901, 41.840049}, {2914.737193, 41.331624},
+ {2916.791072, 40.823533}, {2918.819555, 40.315773},
+ {2920.822658, 39.808340}, {2922.800397, 39.301231},
+ {2924.752789, 38.794442}, {2926.679849, 38.287969},
+ {2928.581593, 37.781809}, {2930.458038, 37.275958},
+ {2932.309197, 36.770412}, {2934.135086, 36.265169},
+ {2935.935721, 35.760225}, {2937.711116, 35.255576},
+ {2939.461286, 34.751219}, {2941.186245, 34.247150},
+ {2942.886008, 33.743367}, {2944.560589, 33.239865},
+ {2946.210002, 32.736642}, {2947.834260, 32.233693},
+ {2949.433378, 31.731017}, {2951.007369, 31.228610},
+ {2952.556245, 30.726467}, {2954.080022, 30.224587},
+ {2955.578711, 29.722966}, {2957.052325, 29.221601},
+ {2958.500877, 28.720488}, {2959.924380, 28.219625},
+ {2961.322846, 27.719009}, {2962.696287, 27.218636},
+ {2964.044715, 26.718503}, {2965.368143, 26.218609},
+ {2966.666582, 25.718948}, {2967.940044, 25.219520},
+ {2969.188540, 24.720320}, {2970.412081, 24.221346},
+ {2971.610680, 23.722595}, {2972.784346, 23.224065},
+ {2973.933092, 22.725752}, {2975.056927, 22.227653},
+ {2976.155862, 21.729767}, {2977.229909, 21.232091},
+ {2978.279077, 20.734621}, {2979.303376, 20.237356},
+ {2980.302817, 19.740292}, {2981.277410, 19.243428},
+ {2982.227165, 18.746760}, {2983.152091, 18.250287},
+ {2984.052199, 17.754006}, {2984.927497, 17.257915},
+ {2985.777995, 16.762011}, {2986.603702, 16.266292},
+ {2987.404629, 15.770756}, {2988.180782, 15.275402},
+ {2988.932173, 14.780225}, {2989.658809, 14.285226},
+ {2990.360700, 13.790402}, {2991.037854, 13.295750},
+ {2991.690279, 12.801269}, {2992.317985, 12.306957},
+ {2992.920979, 11.812813}, {2993.499270, 11.318834},
+ {2994.052867, 10.825019}, {2994.581776, 10.331367},
+ {2995.086007, 9.837875}, {2995.565568, 9.344543},
+ {2996.020466, 8.851368}, {2996.450709, 8.358350},
+ {2996.856305, 7.865487}, {2997.237261, 7.372779},
+ {2997.593586, 6.880223}, {2997.925287, 6.387819},
+ {2998.232372, 5.895565}, {2998.514848, 5.403462},
+ {2998.772722, 4.911507}, {2999.006002, 4.419700},
+ {2999.214696, 3.928041}, {2999.398810, 3.436528},
+ {2999.558352, 2.945161}, {2999.693330, 2.453940},
+ {2999.803750, 1.962863}, {2999.889620, 1.471931},
+ {2999.950946, 0.981144}, {2999.987737, 0.490500},
+ {2999.998745, 0.156943}, {2999.999950, 0.031348},
+ {2999.999998, 0.006229}, {3000.000000, 0.001206},
+ {3000.000000, 0.000201}, {3000.000000, 0.000000},
+ }},
+ {395,
+ {
+ {905.722649, 250.000000}, {905.722649, 250.000000},
+ {905.722649, 250.000000}, {915.489801, 249.123798},
+ {927.918178, 248.011271}, {940.291097, 246.905506},
+ {952.608895, 245.806396}, {964.871901, 244.713839},
+ {977.080440, 243.627733}, {989.234833, 242.547983},
+ {1001.335395, 241.474492}, {1013.382436, 240.407170},
+ {1025.376264, 239.345927}, {1037.317179, 238.290676},
+ {1049.205479, 237.241332}, {1061.041458, 236.197812},
+ {1072.825404, 235.160037}, {1084.557603, 234.127926},
+ {1096.238336, 233.101404}, {1107.867881, 232.080396},
+ {1119.446512, 231.064828}, {1130.974498, 230.054630},
+ {1142.452107, 229.049730}, {1153.879602, 228.050061},
+ {1165.257242, 227.055556}, {1176.585285, 226.066149},
+ {1187.863983, 225.081776}, {1199.093587, 224.102374},
+ {1210.274343, 223.127882}, {1221.406496, 222.158239},
+ {1232.490287, 221.193387}, {1243.525953, 220.233266},
+ {1254.513731, 219.277821}, {1265.453851, 218.326995},
+ {1276.346544, 217.380733}, {1287.192037, 216.438981},
+ {1297.990554, 215.501688}, {1308.742316, 214.568799},
+ {1319.447543, 213.640265}, {1330.106450, 212.716036},
+ {1340.719252, 211.796061}, {1351.286161, 210.880293},
+ {1361.807386, 209.968683}, {1372.283132, 209.061186},
+ {1382.713606, 208.157753}, {1393.099008, 207.258341},
+ {1403.439539, 206.362905}, {1413.735397, 205.471400},
+ {1423.986777, 204.583782}, {1434.193871, 203.700011},
+ {1444.356873, 202.820043}, {1454.475970, 201.943837},
+ {1464.551350, 201.071352}, {1474.583197, 200.202549},
+ {1484.571695, 199.337388}, {1494.517026, 198.475829},
+ {1504.419368, 197.617835}, {1514.278898, 196.763368},
+ {1524.095792, 195.912390}, {1533.870223, 195.064864},
+ {1543.602363, 194.220755}, {1553.292383, 193.380027},
+ {1562.940450, 192.542645}, {1572.546730, 191.708573},
+ {1582.111389, 190.877778}, {1591.634589, 190.050226},
+ {1601.116492, 189.225883}, {1610.557257, 188.404717},
+ {1619.957042, 187.586696}, {1629.316004, 186.771787},
+ {1638.634298, 185.959960}, {1647.912076, 185.151182},
+ {1657.149491, 184.345423}, {1666.346693, 183.542653},
+ {1675.503831, 182.742843}, {1684.621051, 181.945962},
+ {1693.698499, 181.151981}, {1702.736321, 180.360873},
+ {1711.734658, 179.572607}, {1720.693652, 178.787158},
+ {1729.613443, 178.004496}, {1738.494171, 177.224596},
+ {1747.335971, 176.447429}, {1756.138981, 175.672969},
+ {1764.903335, 174.901191}, {1773.629167, 174.132067},
+ {1782.316608, 173.365574}, {1790.965789, 172.601684},
+ {1799.576841, 171.840375}, {1808.149890, 171.081620},
+ {1816.685066, 170.325395}, {1825.182493, 169.571678},
+ {1833.642296, 168.820443}, {1842.064598, 168.071668},
+ {1850.449523, 167.325329}, {1858.797192, 166.581404},
+ {1867.107724, 165.839870}, {1875.381238, 165.100704},
+ {1883.617853, 164.363886}, {1891.817685, 163.629392},
+ {1899.980849, 162.897201}, {1908.107462, 162.167293},
+ {1916.197635, 161.439647}, {1924.251482, 160.714241},
+ {1932.269115, 159.991054}, {1940.250643, 159.270068},
+ {1948.196176, 158.551262}, {1956.105823, 157.834615},
+ {1963.979691, 157.120110}, {1971.817887, 156.407725},
+ {1979.620516, 155.697443}, {1987.387683, 154.989244},
+ {1995.119492, 154.283110}, {2002.816046, 153.579022},
+ {2010.477445, 152.876961}, {2018.103792, 152.176911},
+ {2025.695186, 151.478854}, {2033.251727, 150.782771},
+ {2040.773512, 150.088645}, {2048.260640, 149.396459},
+ {2055.713206, 148.706196}, {2063.131307, 148.017839},
+ {2070.515037, 147.331372}, {2077.864491, 146.646778},
+ {2085.179761, 145.964041}, {2092.460941, 145.283144},
+ {2099.708121, 144.604072}, {2106.921393, 143.926809},
+ {2114.100847, 143.251340}, {2121.246572, 142.577649},
+ {2128.358656, 141.905721}, {2135.437188, 141.235540},
+ {2142.482253, 140.567092}, {2149.493940, 139.900362},
+ {2156.472332, 139.235336}, {2163.417516, 138.571999},
+ {2170.329574, 137.910336}, {2177.208591, 137.250334},
+ {2184.054649, 136.591979}, {2190.867829, 135.935256},
+ {2197.648215, 135.280152}, {2204.395885, 134.626654},
+ {2211.110920, 133.974748}, {2217.793399, 133.324420},
+ {2224.443401, 132.675658}, {2231.061004, 132.028448},
+ {2237.646284, 131.382778}, {2244.199320, 130.738635},
+ {2250.720186, 130.096006}, {2257.208958, 129.454878},
+ {2263.665711, 128.815240}, {2270.090519, 128.177078},
+ {2276.483455, 127.540382}, {2282.844593, 126.905138},
+ {2289.174005, 126.271335}, {2295.471762, 125.638961},
+ {2301.737937, 125.008004}, {2307.972598, 124.378453},
+ {2314.175817, 123.750296}, {2320.347662, 123.123522},
+ {2326.488203, 122.498119}, {2332.597508, 121.874077},
+ {2338.675645, 121.251384}, {2344.722680, 120.630029},
+ {2350.738681, 120.010002}, {2356.723713, 119.391292},
+ {2362.677843, 118.773888}, {2368.601134, 118.157779},
+ {2374.493653, 117.542956}, {2380.355462, 116.929407},
+ {2386.186625, 116.317122}, {2391.987205, 115.706092},
+ {2397.757265, 115.096305}, {2403.496867, 114.487753},
+ {2409.206071, 113.880425}, {2414.884939, 113.274310},
+ {2420.533532, 112.669401}, {2426.151909, 112.065686},
+ {2431.740130, 111.463156}, {2437.298254, 110.861802},
+ {2442.826340, 110.261614}, {2448.324445, 109.662583},
+ {2453.792627, 109.064699}, {2459.230943, 108.467954},
+ {2464.639450, 107.872338}, {2470.018205, 107.277842},
+ {2475.367262, 106.684457}, {2480.686678, 106.092174},
+ {2485.976507, 105.500985}, {2491.236804, 104.910880},
+ {2496.467622, 104.321851}, {2501.669016, 103.733890},
+ {2506.841037, 103.146987}, {2511.983740, 102.561134},
+ {2517.097177, 101.976323}, {2522.181399, 101.392545},
+ {2527.236457, 100.809792}, {2532.262403, 100.228056},
+ {2537.259288, 99.647328}, {2542.227161, 99.067601},
+ {2547.166073, 98.488866}, {2552.076072, 97.911116},
+ {2556.957209, 97.334342}, {2561.809531, 96.758536},
+ {2566.633086, 96.183691}, {2571.427924, 95.609799},
+ {2576.194090, 95.036853}, {2580.931632, 94.464843},
+ {2585.640598, 93.893764}, {2590.321032, 93.323607},
+ {2594.972981, 92.754365}, {2599.596491, 92.186030},
+ {2604.191607, 91.618595}, {2608.758373, 91.052053},
+ {2613.296834, 90.486396}, {2617.807034, 89.921617},
+ {2622.289017, 89.357709}, {2626.742827, 88.794664},
+ {2631.168505, 88.232476}, {2635.566096, 87.671138},
+ {2639.935640, 87.110643}, {2644.277181, 86.550983},
+ {2648.590759, 85.992151}, {2652.876416, 85.434142},
+ {2657.134194, 84.876947}, {2661.364131, 84.320560},
+ {2665.566270, 83.764975}, {2669.740649, 83.210185},
+ {2673.887308, 82.656183}, {2678.006287, 82.102962},
+ {2682.097623, 81.550516}, {2686.161357, 80.998839},
+ {2690.197526, 80.447923}, {2694.206169, 79.897763},
+ {2698.187321, 79.348352}, {2702.141022, 78.799684},
+ {2706.067308, 78.251752}, {2709.966216, 77.704550},
+ {2713.837781, 77.158072}, {2717.682041, 76.612312},
+ {2721.499030, 76.067263}, {2725.288785, 75.522919},
+ {2729.051340, 74.979274}, {2732.786730, 74.436323},
+ {2736.494989, 73.894059}, {2740.176153, 73.352476},
+ {2743.830254, 72.811568}, {2747.457326, 72.271329},
+ {2751.057403, 71.731754}, {2754.630518, 71.192836},
+ {2758.176703, 70.654570}, {2761.695991, 70.116950},
+ {2765.188414, 69.579970}, {2768.654004, 69.043625},
+ {2772.092792, 68.507908}, {2775.504810, 67.972815},
+ {2778.890089, 67.438339}, {2782.248660, 66.904475},
+ {2785.580552, 66.371217}, {2788.885796, 65.838560},
+ {2792.164423, 65.306499}, {2795.416461, 64.775028},
+ {2798.641940, 64.244141}, {2801.840889, 63.713833},
+ {2805.013338, 63.184099}, {2808.159314, 62.654933},
+ {2811.278845, 62.126331}, {2814.371961, 61.598286},
+ {2817.438688, 61.070794}, {2820.479054, 60.543849},
+ {2823.493086, 60.017447}, {2826.480812, 59.491581},
+ {2829.442257, 58.966248}, {2832.377450, 58.441441},
+ {2835.286415, 57.917156}, {2838.169178, 57.393388},
+ {2841.025766, 56.870131}, {2843.856204, 56.347381},
+ {2846.660517, 55.825133}, {2849.438730, 55.303382},
+ {2852.190867, 54.782122}, {2854.916954, 54.261350},
+ {2857.617014, 53.741059}, {2860.291072, 53.221247},
+ {2862.939151, 52.701906}, {2865.561274, 52.183034},
+ {2868.157466, 51.664624}, {2870.727748, 51.146673},
+ {2873.272144, 50.629176}, {2875.790677, 50.112127},
+ {2878.283368, 49.595523}, {2880.750240, 49.079359},
+ {2883.191315, 48.563630}, {2885.606614, 48.048331},
+ {2887.996159, 47.533459}, {2890.359970, 47.019008},
+ {2892.698070, 46.504974}, {2895.010478, 45.991353},
+ {2897.297215, 45.478140}, {2899.558302, 44.965331},
+ {2901.793759, 44.452921}, {2904.003604, 43.940907},
+ {2906.187859, 43.429283}, {2908.346542, 42.918046},
+ {2910.479673, 42.407190}, {2912.587271, 41.896713},
+ {2914.669354, 41.386610}, {2916.725941, 40.876876},
+ {2918.757050, 40.367508}, {2920.762701, 39.858501},
+ {2922.742909, 39.349851}, {2924.697695, 38.841554},
+ {2926.627074, 38.333606}, {2928.531064, 37.826003},
+ {2930.409682, 37.318740}, {2932.262946, 36.811815},
+ {2934.090872, 36.305223}, {2935.893477, 35.798960},
+ {2937.670776, 35.293022}, {2939.422787, 34.787406},
+ {2941.149525, 34.282107}, {2942.851006, 33.777121},
+ {2944.527245, 33.272446}, {2946.178258, 32.768077},
+ {2947.804060, 32.264010}, {2949.404666, 31.760241},
+ {2950.980091, 31.256768}, {2952.530350, 30.753586},
+ {2954.055457, 30.250692}, {2955.555427, 29.748082},
+ {2957.030272, 29.245753}, {2958.480009, 28.743701},
+ {2959.904649, 28.241922}, {2961.304208, 27.740414},
+ {2962.678697, 27.239173}, {2964.028132, 26.738195},
+ {2965.352523, 26.237477}, {2966.651886, 25.737016},
+ {2967.926231, 25.236808}, {2969.175573, 24.736851},
+ {2970.399923, 24.237140}, {2971.599293, 23.737674},
+ {2972.773696, 23.238448}, {2973.923144, 22.739460},
+ {2975.047648, 22.240707}, {2976.147220, 21.742185},
+ {2977.221872, 21.243892}, {2978.271615, 20.745825},
+ {2979.296460, 20.247981}, {2980.296419, 19.750357},
+ {2981.271501, 19.252950}, {2982.221719, 18.755757},
+ {2983.147082, 18.258777}, {2984.047602, 17.762005},
+ {2984.923288, 17.265440}, {2985.774151, 16.769080},
+ {2986.600201, 16.272921}, {2987.401448, 15.776961},
+ {2988.177902, 15.281197}, {2988.929573, 14.785629},
+ {2989.656470, 14.290252}, {2990.358603, 13.795065},
+ {2991.035981, 13.300067}, {2991.688614, 12.805254},
+ {2992.316511, 12.310624}, {2992.919681, 11.816176},
+ {2993.498133, 11.321908}, {2994.051876, 10.827818},
+ {2994.580919, 10.333905}, {2995.085271, 9.840165},
+ {2995.564940, 9.346599}, {2996.019935, 8.853203},
+ {2996.450265, 8.359978}, {2996.855937, 7.866920},
+ {2997.236961, 7.374030}, {2997.593344, 6.881305},
+ {2997.925095, 6.388745}, {2998.232223, 5.896347},
+ {2998.514734, 5.404112}, {2998.772638, 4.912038},
+ {2999.005942, 4.420125}, {2999.214654, 3.928371},
+ {2999.398783, 3.436775}, {2999.558336, 2.945337},
+ {2999.693321, 2.454057}, {2999.803746, 1.962934},
+ {2999.889618, 1.471967}, {2999.950946, 0.981155},
+ {2999.987737, 0.490500}, {2999.998745, 0.156943},
+ {2999.999950, 0.031348}, {2999.999998, 0.006229},
+ {3000.000000, 0.001206}, {3000.000000, 0.000201},
+ {3000.000000, 0.000000},
+ }},
+ {385,
+ {
+ {986.524002, 250.000000}, {986.524002, 250.000000},
+ {991.725533, 249.488842}, {1004.169530, 248.271036},
+ {1016.552865, 247.062377}, {1028.875992, 245.862696},
+ {1041.139355, 244.671826}, {1053.343391, 243.489608},
+ {1065.488528, 242.315888}, {1077.575188, 241.150514},
+ {1089.603784, 239.993341}, {1101.574724, 238.844228},
+ {1113.488405, 237.703035}, {1125.345222, 236.569631},
+ {1137.145560, 235.443884}, {1148.889799, 234.325668},
+ {1160.578312, 233.214860}, {1172.211467, 232.111339},
+ {1183.789625, 231.014989}, {1195.313142, 229.925695},
+ {1206.782368, 228.843347}, {1218.197648, 227.767836},
+ {1229.559320, 226.699056}, {1240.867719, 225.636904},
+ {1252.123174, 224.581280}, {1263.326008, 223.532083},
+ {1274.476540, 222.489220}, {1285.575086, 221.452595},
+ {1296.621953, 220.422116}, {1307.617449, 219.397694},
+ {1318.561872, 218.379240}, {1329.455520, 217.366670},
+ {1340.298684, 216.359898}, {1351.091652, 215.358842},
+ {1361.834709, 214.363421}, {1372.528134, 213.373558},
+ {1383.172202, 212.389173}, {1393.767186, 211.410192},
+ {1404.313354, 210.436540}, {1414.810971, 209.468145},
+ {1425.260298, 208.504934}, {1435.661593, 207.546839},
+ {1446.015108, 206.593791}, {1456.321096, 205.645721},
+ {1466.579803, 204.702566}, {1476.791474, 203.764259},
+ {1486.956349, 202.830737}, {1497.074666, 201.901938},
+ {1507.146659, 200.977801}, {1517.172561, 200.058266},
+ {1527.152599, 199.143274}, {1537.087001, 198.232767},
+ {1546.975987, 197.326688}, {1556.819779, 196.424982},
+ {1566.618593, 195.527594}, {1576.372645, 194.634469},
+ {1586.082145, 193.745555}, {1595.747304, 192.860801},
+ {1605.368328, 191.980154}, {1614.945421, 191.103566},
+ {1624.478785, 190.230985}, {1633.968619, 189.362366},
+ {1643.415119, 188.497658}, {1652.818481, 187.636816},
+ {1662.178896, 186.779794}, {1671.496555, 185.926546},
+ {1680.771644, 185.077028}, {1690.004350, 184.231196},
+ {1699.194855, 183.389007}, {1708.343340, 182.550419},
+ {1717.449986, 181.715389}, {1726.514967, 180.883877},
+ {1735.538460, 180.055843}, {1744.520638, 179.231247},
+ {1753.461670, 178.410049}, {1762.361726, 177.592212},
+ {1771.220974, 176.777697}, {1780.039578, 175.966468},
+ {1788.817702, 175.158487}, {1797.555507, 174.353718},
+ {1806.253153, 173.552127}, {1814.910799, 172.753677},
+ {1823.528599, 171.958335}, {1832.106709, 171.166067},
+ {1840.645282, 170.376839}, {1849.144468, 169.590618},
+ {1857.604418, 168.807372}, {1866.025279, 168.027069},
+ {1874.407197, 167.249677}, {1882.750319, 166.475166},
+ {1891.054785, 165.703504}, {1899.320739, 164.934663},
+ {1907.548321, 164.168612}, {1915.737670, 163.405322},
+ {1923.888922, 162.644764}, {1932.002214, 161.886910},
+ {1940.077680, 161.131732}, {1948.115453, 160.379203},
+ {1956.115666, 159.629294}, {1964.078447, 158.881980},
+ {1972.003928, 158.137234}, {1979.892234, 157.395030},
+ {1987.743494, 156.655342}, {1995.557831, 155.918145},
+ {2003.335370, 155.183414}, {2011.076233, 154.451124},
+ {2018.780543, 153.721252}, {2026.448418, 152.993772},
+ {2034.079979, 152.268662}, {2041.675343, 151.545898},
+ {2049.234627, 150.825457}, {2056.757946, 150.107317},
+ {2064.245416, 149.391454}, {2071.697148, 148.677848},
+ {2079.113256, 147.966475}, {2086.493851, 147.257315},
+ {2093.839043, 146.550347}, {2101.148940, 145.845549},
+ {2108.423651, 145.142900}, {2115.663283, 144.442380},
+ {2122.867942, 143.743970}, {2130.037732, 143.047648},
+ {2137.172758, 142.353396}, {2144.273123, 141.661194},
+ {2151.338929, 140.971023}, {2158.370276, 140.282863},
+ {2165.367265, 139.596697}, {2172.329995, 138.912505},
+ {2179.258564, 138.230269}, {2186.153070, 137.549972},
+ {2193.013609, 136.871596}, {2199.840277, 136.195122},
+ {2206.633169, 135.520533}, {2213.392377, 134.847813},
+ {2220.117996, 134.176944}, {2226.810118, 133.507910},
+ {2233.468833, 132.840693}, {2240.094232, 132.175278},
+ {2246.686405, 131.511648}, {2253.245441, 130.849788},
+ {2259.771428, 130.189681}, {2266.264453, 129.531311},
+ {2272.724602, 128.874664}, {2279.151962, 128.219723},
+ {2285.546617, 127.566474}, {2291.908651, 126.914902},
+ {2298.238148, 126.264992}, {2304.535191, 125.616729},
+ {2310.799862, 124.970099}, {2317.032242, 124.325087},
+ {2323.232411, 123.681679}, {2329.400449, 123.039861},
+ {2335.536436, 122.399620}, {2341.640450, 121.760941},
+ {2347.712569, 121.123811}, {2353.752870, 120.488217},
+ {2359.761429, 119.854144}, {2365.738322, 119.221581},
+ {2371.683624, 118.590513}, {2377.597410, 117.960928},
+ {2383.479754, 117.332813}, {2389.330728, 116.706156},
+ {2395.150406, 116.080944}, {2400.938858, 115.457164},
+ {2406.696158, 114.834805}, {2412.422374, 114.213853},
+ {2418.117578, 113.594297}, {2423.781838, 112.976126},
+ {2429.415225, 112.359326}, {2435.017805, 111.743888},
+ {2440.589647, 111.129797}, {2446.130818, 110.517045},
+ {2451.641385, 109.905618}, {2457.121413, 109.295506},
+ {2462.570968, 108.686697}, {2467.990115, 108.079181},
+ {2473.378918, 107.472946}, {2478.737441, 106.867982},
+ {2484.065748, 106.264278}, {2489.363900, 105.661823},
+ {2494.631961, 105.060606}, {2499.869992, 104.460617},
+ {2505.078053, 103.861845}, {2510.256206, 103.264281},
+ {2515.404511, 102.667914}, {2520.523027, 102.072734},
+ {2525.611814, 101.478730}, {2530.670930, 100.885894},
+ {2535.700432, 100.294214}, {2540.700380, 99.703681},
+ {2545.670829, 99.114286}, {2550.611836, 98.526018},
+ {2555.523459, 97.938869}, {2560.405751, 97.352828},
+ {2565.258769, 96.767887}, {2570.082567, 96.184035},
+ {2574.877199, 95.601264}, {2579.642720, 95.019565},
+ {2584.379183, 94.438928}, {2589.086639, 93.859344},
+ {2593.765143, 93.280804}, {2598.414746, 92.703300},
+ {2603.035499, 92.126823}, {2607.627453, 91.551363},
+ {2612.190660, 90.976912}, {2616.725170, 90.403462},
+ {2621.231031, 89.831004}, {2625.708295, 89.259529},
+ {2630.157008, 88.689028}, {2634.577222, 88.119495},
+ {2638.968982, 87.550919}, {2643.332337, 86.983294},
+ {2647.667335, 86.416610}, {2651.974022, 85.850860},
+ {2656.252444, 85.286035}, {2660.502648, 84.722128},
+ {2664.724679, 84.159130}, {2668.918584, 83.597034},
+ {2673.084405, 83.035831}, {2677.222189, 82.475514},
+ {2681.331979, 81.916076}, {2685.413818, 81.357508},
+ {2689.467751, 80.799803}, {2693.493820, 80.242953},
+ {2697.492067, 79.686950}, {2701.462536, 79.131788},
+ {2705.405267, 78.577459}, {2709.320302, 78.023955},
+ {2713.207683, 77.471269}, {2717.067450, 76.919394},
+ {2720.899642, 76.368322}, {2724.704302, 75.818047},
+ {2728.481467, 75.268560}, {2732.231177, 74.719856},
+ {2735.953472, 74.171927}, {2739.648389, 73.624766},
+ {2743.315967, 73.078365}, {2746.956244, 72.532719},
+ {2750.569258, 71.987821}, {2754.155045, 71.443662},
+ {2757.713643, 70.900237}, {2761.245087, 70.357540},
+ {2764.749415, 69.815562}, {2768.226661, 69.274298},
+ {2771.676862, 68.733741}, {2775.100053, 68.193884},
+ {2778.496268, 67.654721}, {2781.865542, 67.116245},
+ {2785.207909, 66.578450}, {2788.523404, 66.041330},
+ {2791.812059, 65.504878}, {2795.073908, 64.969088},
+ {2798.308984, 64.433953}, {2801.517320, 63.899467},
+ {2804.698947, 63.365624}, {2807.853898, 62.832418},
+ {2810.982204, 62.299843}, {2814.083898, 61.767893},
+ {2817.159009, 61.236561}, {2820.207569, 60.705841},
+ {2823.229609, 60.175728}, {2826.225157, 59.646216},
+ {2829.194245, 59.117298}, {2832.136902, 58.588970},
+ {2835.053157, 58.061224}, {2837.943038, 57.534055},
+ {2840.806576, 57.007457}, {2843.643798, 56.481425},
+ {2846.454733, 55.955953}, {2849.239407, 55.431035},
+ {2851.997850, 54.906666}, {2854.730088, 54.382840},
+ {2857.436147, 53.859551}, {2860.116056, 53.336794},
+ {2862.769840, 52.814563}, {2865.397525, 52.292854},
+ {2867.999138, 51.771659}, {2870.574704, 51.250975},
+ {2873.124248, 50.730796}, {2875.647796, 50.211116},
+ {2878.145372, 49.691930}, {2880.617001, 49.173233},
+ {2883.062708, 48.655019}, {2885.482515, 48.137284},
+ {2887.876448, 47.620021}, {2890.244529, 47.103227},
+ {2892.586782, 46.586896}, {2894.903230, 46.071023},
+ {2897.193896, 45.555602}, {2899.458802, 45.040629},
+ {2901.697970, 44.526099}, {2903.911422, 44.012007},
+ {2906.099181, 43.498348}, {2908.261268, 42.985116},
+ {2910.397703, 42.472308}, {2912.508509, 41.959918},
+ {2914.593706, 41.447942}, {2916.653314, 40.936375},
+ {2918.687353, 40.425212}, {2920.695845, 39.914448},
+ {2922.678808, 39.404078}, {2924.636262, 38.894099},
+ {2926.568227, 38.384505}, {2928.474722, 37.875293},
+ {2930.355766, 37.366456}, {2932.211377, 36.857991},
+ {2934.041574, 36.349894}, {2935.846376, 35.842160},
+ {2937.625799, 35.334784}, {2939.379863, 34.827763},
+ {2941.108584, 34.321091}, {2942.811981, 33.814765},
+ {2944.490069, 33.308780}, {2946.142867, 32.803132},
+ {2947.770391, 32.297817}, {2949.372657, 31.792830},
+ {2950.949682, 31.288168}, {2952.501482, 30.783826},
+ {2954.028072, 30.279801}, {2955.529470, 29.776088},
+ {2957.005689, 29.272684}, {2958.456746, 28.769584},
+ {2959.882655, 28.266784}, {2961.283432, 27.764281},
+ {2962.659090, 27.262071}, {2964.009646, 26.760150},
+ {2965.335113, 26.258514}, {2966.635504, 25.757160},
+ {2967.910835, 25.256083}, {2969.161120, 24.755281},
+ {2970.386370, 24.254749}, {2971.586601, 23.754485},
+ {2972.761825, 23.254484}, {2973.912056, 22.754743},
+ {2975.037306, 22.255259}, {2976.137588, 21.756029},
+ {2977.212915, 21.257048}, {2978.263299, 20.758315},
+ {2979.288753, 20.259825}, {2980.289288, 19.761575},
+ {2981.264916, 19.263563}, {2982.215650, 18.765785},
+ {2983.141501, 18.268239}, {2984.042480, 17.770921},
+ {2984.918598, 17.273828}, {2985.769868, 16.776958},
+ {2986.596300, 16.280308}, {2987.397904, 15.783875},
+ {2988.174692, 15.287657}, {2988.926675, 14.791650},
+ {2989.653863, 14.295853}, {2990.356266, 13.800263},
+ {2991.033894, 13.304877}, {2991.686758, 12.809694},
+ {2992.314868, 12.314710}, {2992.918234, 11.819924},
+ {2993.496866, 11.325334}, {2994.050773, 10.830937},
+ {2994.579964, 10.336732}, {2995.084451, 9.842717},
+ {2995.564241, 9.348889}, {2996.019344, 8.855248},
+ {2996.449770, 8.361791}, {2996.855528, 7.868516},
+ {2997.236626, 7.375423}, {2997.593075, 6.882510},
+ {2997.924882, 6.389776}, {2998.232057, 5.897218},
+ {2998.514608, 5.404837}, {2998.772545, 4.912630},
+ {2999.005875, 4.420597}, {2999.214609, 3.928738},
+ {2999.398753, 3.437050}, {2999.558318, 2.945534},
+ {2999.693311, 2.454188}, {2999.803741, 1.963012},
+ {2999.889616, 1.472006}, {2999.950946, 0.981168},
+ {2999.987737, 0.490500}, {2999.998745, 0.156943},
+ {2999.999950, 0.031348}, {2999.999998, 0.006229},
+ {3000.000000, 0.001206}, {3000.000000, 0.000201},
+ {3000.000000, 0.000000},
+ }},
+ {376,
+ {
+ {1065.277806, 250.000000}, {1065.277806, 250.000000},
+ {1066.951150, 249.819943}, {1079.408842, 248.487728},
+ {1091.800225, 247.167577}, {1104.125895, 245.859231},
+ {1116.386437, 244.562438}, {1128.582421, 243.276955},
+ {1140.714409, 242.002548}, {1152.782947, 240.738988},
+ {1164.788574, 239.486057}, {1176.731813, 238.243540},
+ {1188.613183, 237.011230}, {1200.433187, 235.788929},
+ {1212.192321, 234.576440}, {1223.891071, 233.373576},
+ {1235.529914, 232.180153}, {1247.109318, 230.995994},
+ {1258.629741, 229.820925}, {1270.091634, 228.654779},
+ {1281.495438, 227.497393}, {1292.841588, 226.348606},
+ {1304.130510, 225.208266}, {1315.362622, 224.076221},
+ {1326.538336, 222.952324}, {1337.658055, 221.836434},
+ {1348.722176, 220.728411}, {1359.731089, 219.628120},
+ {1370.685178, 218.535429}, {1381.584819, 217.450208},
+ {1392.430382, 216.372333}, {1403.222232, 215.301680},
+ {1413.960728, 214.238132}, {1424.646220, 213.181570},
+ {1435.279057, 212.131881}, {1445.859577, 211.088954},
+ {1456.388118, 210.052680}, {1466.865009, 209.022953},
+ {1477.290575, 207.999671}, {1487.665135, 206.982731},
+ {1497.989004, 205.972035}, {1508.262492, 204.967485},
+ {1518.485904, 203.968988}, {1528.659540, 202.976452},
+ {1538.783696, 201.989785}, {1548.858663, 201.008899},
+ {1558.884728, 200.033708}, {1568.862174, 199.064127},
+ {1578.791279, 198.100074}, {1588.672317, 197.141467},
+ {1598.505560, 196.188226}, {1608.291272, 195.240275},
+ {1618.029718, 194.297537}, {1627.721154, 193.359938},
+ {1637.365838, 192.427405}, {1646.964020, 191.499865},
+ {1656.515948, 190.577250}, {1666.021866, 189.659490},
+ {1675.482016, 188.746519}, {1684.896636, 187.838270},
+ {1694.265960, 186.934679}, {1703.590219, 186.035682},
+ {1712.869641, 185.141218}, {1722.104452, 184.251225},
+ {1731.294874, 183.365644}, {1740.441126, 182.484417},
+ {1749.543423, 181.607485}, {1758.601980, 180.734792},
+ {1767.617007, 179.866284}, {1776.588712, 179.001905},
+ {1785.517300, 178.141604}, {1794.402973, 177.285326},
+ {1803.245931, 176.433022}, {1812.046373, 175.584641},
+ {1820.804492, 174.740133}, {1829.520482, 173.899449},
+ {1838.194532, 173.062543}, {1846.826830, 172.229368},
+ {1855.417561, 171.399877}, {1863.966908, 170.574025},
+ {1872.475053, 169.751768}, {1880.942174, 168.933062},
+ {1889.368447, 168.117865}, {1897.754047, 167.306135},
+ {1906.099146, 166.497829}, {1914.403914, 165.692908},
+ {1922.668520, 164.891332}, {1930.893130, 164.093061},
+ {1939.077908, 163.298057}, {1947.223017, 162.506281},
+ {1955.328616, 161.717697}, {1963.394865, 160.932268},
+ {1971.421921, 160.149957}, {1979.409938, 159.370730},
+ {1987.359070, 158.594550}, {1995.269468, 157.821385},
+ {2003.141283, 157.051199}, {2010.974662, 156.283960},
+ {2018.769752, 155.519636}, {2026.526698, 154.758193},
+ {2034.245642, 153.999600}, {2041.926728, 153.243826},
+ {2049.570095, 152.490840}, {2057.175881, 151.740613},
+ {2064.744224, 150.993113}, {2072.275260, 150.248312},
+ {2079.769122, 149.506181}, {2087.225944, 148.766692},
+ {2094.645857, 148.029816}, {2102.028990, 147.295525},
+ {2109.375473, 146.563794}, {2116.685433, 145.834594},
+ {2123.958995, 145.107900}, {2131.196285, 144.383685},
+ {2138.397425, 143.661924}, {2145.562538, 142.942591},
+ {2152.691744, 142.225662}, {2159.785164, 141.511112},
+ {2166.842914, 140.798917}, {2173.865114, 140.089053},
+ {2180.851877, 139.381496}, {2187.803320, 138.676223},
+ {2194.719556, 137.973212}, {2201.600698, 137.272440},
+ {2208.446856, 136.573883}, {2215.258141, 135.877522},
+ {2222.034662, 135.183333}, {2228.776528, 134.491295},
+ {2235.483845, 133.801388}, {2242.156719, 133.113590},
+ {2248.795256, 132.427882}, {2255.399559, 131.744241},
+ {2261.969732, 131.062650}, {2268.505875, 130.383087},
+ {2275.008090, 129.705533}, {2281.476478, 129.029969},
+ {2287.911137, 128.356377}, {2294.312164, 127.684736},
+ {2300.679659, 127.015029}, {2307.013715, 126.347237},
+ {2313.314430, 125.681343}, {2319.581897, 125.017327},
+ {2325.816209, 124.355174}, {2332.017460, 123.694864},
+ {2338.185741, 123.036381}, {2344.321143, 122.379708},
+ {2350.423757, 121.724827}, {2356.493670, 121.071723},
+ {2362.530973, 120.420379}, {2368.535752, 119.770778},
+ {2374.508094, 119.122904}, {2380.448085, 118.476742},
+ {2386.355811, 117.832276}, {2392.231355, 117.189490},
+ {2398.074801, 116.548369}, {2403.886233, 115.908897},
+ {2409.665732, 115.271059}, {2415.413379, 114.634841},
+ {2421.129256, 114.000228}, {2426.813442, 113.367206},
+ {2432.466016, 112.735759}, {2438.087057, 112.105874},
+ {2443.676642, 111.477536}, {2449.234849, 110.850731},
+ {2454.761753, 110.225447}, {2460.257431, 109.601668},
+ {2465.721957, 108.979382}, {2471.155406, 108.358574},
+ {2476.557851, 107.739233}, {2481.929366, 107.121344},
+ {2487.270022, 106.504894}, {2492.579891, 105.889871},
+ {2497.859044, 105.276262}, {2503.107552, 104.664055},
+ {2508.325484, 104.053235}, {2513.512910, 103.443793},
+ {2518.669898, 102.835714}, {2523.796515, 102.228987},
+ {2528.892830, 101.623600}, {2533.958908, 101.019541},
+ {2538.994817, 100.416798}, {2544.000621, 99.815359},
+ {2548.976385, 99.215213}, {2553.922174, 98.616348},
+ {2558.838052, 98.018753}, {2563.724081, 97.422416},
+ {2568.580325, 96.827326}, {2573.406844, 96.233473},
+ {2578.203702, 95.640845}, {2582.970959, 95.049430},
+ {2587.708676, 94.459220}, {2592.416911, 93.870201},
+ {2597.095725, 93.282365}, {2601.745177, 92.695700},
+ {2606.365324, 92.110197}, {2610.956225, 91.525843},
+ {2615.517937, 90.942630}, {2620.050517, 90.360547},
+ {2624.554020, 89.779584}, {2629.028503, 89.199731},
+ {2633.474021, 88.620978}, {2637.890628, 88.043315},
+ {2642.278379, 87.466733}, {2646.637328, 86.891220},
+ {2650.967528, 86.316769}, {2655.269031, 85.743369},
+ {2659.541891, 85.171011}, {2663.786158, 84.599685},
+ {2668.001885, 84.029383}, {2672.189122, 83.460094},
+ {2676.347919, 82.891810}, {2680.478327, 82.324521},
+ {2684.580396, 81.758218}, {2688.654174, 81.192893},
+ {2692.699709, 80.628537}, {2696.717051, 80.065140},
+ {2700.706247, 79.502693}, {2704.667344, 78.941189},
+ {2708.600389, 78.380618}, {2712.505429, 77.820972},
+ {2716.382510, 77.262242}, {2720.231676, 76.704420},
+ {2724.052974, 76.147497}, {2727.846448, 75.591465},
+ {2731.612143, 75.036316}, {2735.350101, 74.482041},
+ {2739.060368, 73.928632}, {2742.742986, 73.376081},
+ {2746.397998, 72.824380}, {2750.025445, 72.273522},
+ {2753.625371, 71.723497}, {2757.197816, 71.174298},
+ {2760.742821, 70.625918}, {2764.260428, 70.078348},
+ {2767.750676, 69.531581}, {2771.213606, 68.985609},
+ {2774.649256, 68.440424}, {2778.057667, 67.896019},
+ {2781.438878, 67.352387}, {2784.792925, 66.809519},
+ {2788.119848, 66.267408}, {2791.419685, 65.726048},
+ {2794.692472, 65.185430}, {2797.938246, 64.645547},
+ {2801.157045, 64.106393}, {2804.348903, 63.567959},
+ {2807.513858, 63.030239}, {2810.651945, 62.493226},
+ {2813.763199, 61.956913}, {2816.847654, 61.421292},
+ {2819.905345, 60.886357}, {2822.936306, 60.352101},
+ {2825.940572, 59.818516}, {2828.918175, 59.285597},
+ {2831.869148, 58.753336}, {2834.793524, 58.221726},
+ {2837.691337, 57.690762}, {2840.562617, 57.160436},
+ {2843.407396, 56.630741}, {2846.225706, 56.101671},
+ {2849.017579, 55.573220}, {2851.783044, 55.045382},
+ {2854.522132, 54.518148}, {2857.234873, 53.991515},
+ {2859.921298, 53.465474}, {2862.581436, 52.940019},
+ {2865.215315, 52.415146}, {2867.822964, 51.890846},
+ {2870.404413, 51.367114}, {2872.959690, 50.843944},
+ {2875.488822, 50.321330}, {2877.991837, 49.799265},
+ {2880.468762, 49.277744}, {2882.919624, 48.756761},
+ {2885.344451, 48.236310}, {2887.743269, 47.716384},
+ {2890.116103, 47.196978}, {2892.462979, 46.678086},
+ {2894.783924, 46.159703}, {2897.078962, 45.641822},
+ {2899.348119, 45.124439}, {2901.591418, 44.607546},
+ {2903.808885, 44.091139}, {2906.000544, 43.575212},
+ {2908.166418, 43.059759}, {2910.306532, 42.544775},
+ {2912.420908, 42.030255}, {2914.509569, 41.516193},
+ {2916.572538, 41.002583}, {2918.609838, 40.489421},
+ {2920.621491, 39.976700}, {2922.607519, 39.464416},
+ {2924.567944, 38.952564}, {2926.502786, 38.441137},
+ {2928.412068, 37.930132}, {2930.295810, 37.419542},
+ {2932.154032, 36.909364}, {2933.986756, 36.399591},
+ {2935.794001, 35.890218}, {2937.575788, 35.381242},
+ {2939.332135, 34.872656}, {2941.063063, 34.364456},
+ {2942.768591, 33.856637}, {2944.448736, 33.349195},
+ {2946.103519, 32.842123}, {2947.732958, 32.335419},
+ {2949.337070, 31.829077}, {2950.915874, 31.323092},
+ {2952.469388, 30.817459}, {2953.997629, 30.312175},
+ {2955.500614, 29.807235}, {2956.978361, 29.302634},
+ {2958.430886, 28.798368}, {2959.858206, 28.294432},
+ {2961.260337, 27.790822}, {2962.637296, 27.287534},
+ {2963.989099, 26.784563}, {2965.315761, 26.281906},
+ {2966.617297, 25.779558}, {2967.893724, 25.277514},
+ {2969.145056, 24.775772}, {2970.371309, 24.274327},
+ {2971.572496, 23.773175}, {2972.748633, 23.272311},
+ {2973.899734, 22.771733}, {2975.025814, 22.271437},
+ {2976.126885, 21.771418}, {2977.202962, 21.271673},
+ {2978.254059, 20.772198}, {2979.280189, 20.272990},
+ {2980.281365, 19.774045}, {2981.257600, 19.275360},
+ {2982.208907, 18.776931}, {2983.135299, 18.278755},
+ {2984.036789, 17.780829}, {2984.913388, 17.283150},
+ {2985.765110, 16.785714}, {2986.591966, 16.288517},
+ {2987.393967, 15.791559}, {2988.171127, 15.294834},
+ {2988.923457, 14.798341}, {2989.650967, 14.302076},
+ {2990.353670, 13.806037}, {2991.031576, 13.310222},
+ {2991.684698, 12.814626}, {2992.313044, 12.319249},
+ {2992.916628, 11.824088}, {2993.495459, 11.329139},
+ {2994.049547, 10.834402}, {2994.578904, 10.339873},
+ {2995.083540, 9.845551}, {2995.563464, 9.351433},
+ {2996.018688, 8.857518}, {2996.449221, 8.363804},
+ {2996.855073, 7.870289}, {2997.236255, 7.376971},
+ {2997.592775, 6.883849}, {2997.924644, 6.390920},
+ {2998.231872, 5.898185}, {2998.514468, 5.405641},
+ {2998.772441, 4.913287}, {2999.005801, 4.421122},
+ {2999.214558, 3.929145}, {2999.398720, 3.437355},
+ {2999.558298, 2.945751}, {2999.693300, 2.454333},
+ {2999.803736, 1.963099}, {2999.889615, 1.472049},
+ {2999.950945, 0.981183}, {2999.987737, 0.490500},
+ {2999.998745, 0.156943}, {2999.999950, 0.031348},
+ {2999.999998, 0.006229}, {3000.000000, 0.001206},
+ {3000.000000, 0.000201}, {3000.000000, 0.000000},
+ }},
+ {367,
+ {
+ {1142.065914, 250.000000}, {1142.065914, 250.000000},
+ {1142.065914, 250.000000}, {1153.657448, 248.645856},
+ {1166.053723, 247.205151}, {1178.378344, 245.779673},
+ {1190.632062, 244.369052}, {1202.815612, 242.972931},
+ {1214.929709, 241.590967}, {1226.975054, 240.222826},
+ {1238.952329, 238.868189}, {1250.862203, 237.526747},
+ {1262.705326, 236.198200}, {1274.482338, 234.882259},
+ {1286.193860, 233.578645}, {1297.840504, 232.287087},
+ {1309.422864, 231.007324}, {1320.941525, 229.739101},
+ {1332.397056, 228.482174}, {1343.790018, 227.236304},
+ {1355.120958, 226.001262}, {1366.390410, 224.776822},
+ {1377.598899, 223.562768}, {1388.746941, 222.358890},
+ {1399.835038, 221.164984}, {1410.863684, 219.980850},
+ {1421.833362, 218.806296}, {1432.744548, 217.641134},
+ {1443.597706, 216.485184}, {1454.393292, 215.338268},
+ {1465.131754, 214.200213}, {1475.813531, 213.070854},
+ {1486.439053, 211.950026}, {1497.008743, 210.837572},
+ {1507.523016, 209.733338}, {1517.982278, 208.637173},
+ {1528.386931, 207.548932}, {1538.737366, 206.468473},
+ {1549.033969, 205.395657}, {1559.277120, 204.330348},
+ {1569.467189, 203.272416}, {1579.604542, 202.221732},
+ {1589.689540, 201.178171}, {1599.722534, 200.141611},
+ {1609.703873, 199.111934}, {1619.633897, 198.089024},
+ {1629.512942, 197.072767}, {1639.341337, 196.063053},
+ {1649.119408, 195.059775}, {1658.847473, 194.062826},
+ {1668.525846, 193.072106}, {1678.154837, 192.087512},
+ {1687.734748, 191.108948}, {1697.265880, 190.136318},
+ {1706.748526, 189.169529}, {1716.182977, 188.208489},
+ {1725.569517, 187.253108}, {1734.908427, 186.303301},
+ {1744.199984, 185.358981}, {1753.444460, 184.420065},
+ {1762.642123, 183.486473}, {1771.793238, 182.558123},
+ {1780.898065, 181.634939}, {1789.956859, 180.716844},
+ {1798.969875, 179.803763}, {1807.937359, 178.895624},
+ {1816.859559, 177.992355}, {1825.736715, 177.093886},
+ {1834.569066, 176.200149}, {1843.356846, 175.311076},
+ {1852.100288, 174.426603}, {1860.799620, 173.546664},
+ {1869.455066, 172.671198}, {1878.066850, 171.800142},
+ {1886.635189, 170.933435}, {1895.160301, 170.071020},
+ {1903.642397, 169.212837}, {1912.081689, 168.358830},
+ {1920.478383, 167.508943}, {1928.832685, 166.663121},
+ {1937.144796, 165.821312}, {1945.414915, 164.983462},
+ {1953.643240, 164.149520}, {1961.829963, 163.319435},
+ {1969.975278, 162.493158}, {1978.079373, 161.670641},
+ {1986.142435, 160.851835}, {1994.164648, 160.036693},
+ {2002.146195, 159.225170}, {2010.087255, 158.417221},
+ {2017.988005, 157.612802}, {2025.848622, 156.811868},
+ {2033.669278, 156.014377}, {2041.450145, 155.220287},
+ {2049.191391, 154.429558}, {2056.893183, 153.642148},
+ {2064.555688, 152.858018}, {2072.179066, 152.077129},
+ {2079.763481, 151.299442}, {2087.309090, 150.524920},
+ {2094.816051, 149.753525}, {2102.284519, 148.985222},
+ {2109.714649, 148.219974}, {2117.106592, 147.457747},
+ {2124.460499, 146.698505}, {2131.776517, 145.942214},
+ {2139.054793, 145.188842}, {2146.295473, 144.438354},
+ {2153.498700, 143.690720}, {2160.664615, 142.945906},
+ {2167.793360, 142.203881}, {2174.885072, 141.464615},
+ {2181.939890, 140.728077}, {2188.957948, 139.994237},
+ {2195.939380, 139.263066}, {2202.884320, 138.534534},
+ {2209.792899, 137.808614}, {2216.665246, 137.085277},
+ {2223.501490, 136.364495}, {2230.301759, 135.646241},
+ {2237.066177, 134.930488}, {2243.794869, 134.217209},
+ {2250.487959, 133.506380}, {2257.145568, 132.797973},
+ {2263.767816, 132.091963}, {2270.354824, 131.388327},
+ {2276.906708, 130.687038}, {2283.423585, 129.988072},
+ {2289.905572, 129.291406}, {2296.352783, 128.597017},
+ {2302.765330, 127.904880}, {2309.143327, 127.214973},
+ {2315.486883, 126.527273}, {2321.796109, 125.841758},
+ {2328.071113, 125.158406}, {2334.312003, 124.477194},
+ {2340.518885, 123.798103}, {2346.691865, 123.121109},
+ {2352.831048, 122.446194}, {2358.936536, 121.773335},
+ {2365.008432, 121.102512}, {2371.046838, 120.433707},
+ {2377.051853, 119.766897}, {2383.023577, 119.102065},
+ {2388.962108, 118.439190}, {2394.867544, 117.778253},
+ {2400.739982, 117.119236}, {2406.579516, 116.462120},
+ {2412.386241, 115.806886}, {2418.160251, 115.153517},
+ {2423.901639, 114.501993}, {2429.610496, 113.852298},
+ {2435.286914, 113.204414}, {2440.930982, 112.558324},
+ {2446.542790, 111.914010}, {2452.122427, 111.271455},
+ {2457.669979, 110.630642}, {2463.185534, 109.991555},
+ {2468.669178, 109.354178}, {2474.120995, 108.718495},
+ {2479.541069, 108.084488}, {2484.929485, 107.452143},
+ {2490.286324, 106.821443}, {2495.611670, 106.192373},
+ {2500.905602, 105.564918}, {2506.168202, 104.939063},
+ {2511.399548, 104.314792}, {2516.599720, 103.692090},
+ {2521.768796, 103.070943}, {2526.906853, 102.451337},
+ {2532.013968, 101.833256}, {2537.090216, 101.216687},
+ {2542.135674, 100.601615}, {2547.150415, 99.988026},
+ {2552.134513, 99.375907}, {2557.088042, 98.765243},
+ {2562.011074, 98.156022}, {2566.903680, 97.548229},
+ {2571.765932, 96.941851}, {2576.597900, 96.336876},
+ {2581.399654, 95.733289}, {2586.171263, 95.131079},
+ {2590.912796, 94.530232}, {2595.624320, 93.930735},
+ {2600.305903, 93.332576}, {2604.957611, 92.735742},
+ {2609.579510, 92.140221}, {2614.171666, 91.546001},
+ {2618.734142, 90.953070}, {2623.267005, 90.361415},
+ {2627.770316, 89.771025}, {2632.244138, 89.181887},
+ {2636.688535, 88.593991}, {2641.103568, 88.007323},
+ {2645.489298, 87.421874}, {2649.845786, 86.837631},
+ {2654.173091, 86.254583}, {2658.471274, 85.672720},
+ {2662.740392, 85.092028}, {2666.980506, 84.512499},
+ {2671.191671, 83.934120}, {2675.373946, 83.356881},
+ {2679.527387, 82.780771}, {2683.652051, 82.205780},
+ {2687.747993, 81.631896}, {2691.815268, 81.059110},
+ {2695.853931, 80.487410}, {2699.864036, 79.916787},
+ {2703.845637, 79.347231}, {2707.798786, 78.778730},
+ {2711.723536, 78.211275}, {2715.619939, 77.644857},
+ {2719.488047, 77.079464}, {2723.327911, 76.515087},
+ {2727.139581, 75.951717}, {2730.923107, 75.389344},
+ {2734.678540, 74.827957}, {2738.405928, 74.267548},
+ {2742.105319, 73.708107}, {2745.776762, 73.149625},
+ {2749.420305, 72.592091}, {2753.035995, 72.035498},
+ {2756.623878, 71.479835}, {2760.184002, 70.925094},
+ {2763.716411, 70.371265}, {2767.221151, 69.818340},
+ {2770.698267, 69.266310}, {2774.147804, 68.715165},
+ {2777.569805, 68.164897}, {2780.964315, 67.615497},
+ {2784.331376, 67.066956}, {2787.671032, 66.519266},
+ {2790.983324, 65.972419}, {2794.268295, 65.426405},
+ {2797.525985, 64.881217}, {2800.756437, 64.336846},
+ {2803.959690, 63.793283}, {2807.135785, 63.250521},
+ {2810.284762, 62.708551}, {2813.406660, 62.167365},
+ {2816.501518, 61.626955}, {2819.569374, 61.087313},
+ {2822.610268, 60.548431}, {2825.624236, 60.010301},
+ {2828.611317, 59.472915}, {2831.571546, 58.936266},
+ {2834.504962, 58.400345}, {2837.411599, 57.865146},
+ {2840.291494, 57.330660}, {2843.144682, 56.796880},
+ {2845.971199, 56.263797}, {2848.771079, 55.731406},
+ {2851.544357, 55.199698}, {2854.291066, 54.668666},
+ {2857.011240, 54.138303}, {2859.704913, 53.608601},
+ {2862.372117, 53.079554}, {2865.012885, 52.551153},
+ {2867.627248, 52.023393}, {2870.215240, 51.496265},
+ {2872.776890, 50.969763}, {2875.312231, 50.443879},
+ {2877.821294, 49.918608}, {2880.304107, 49.393942},
+ {2882.760703, 48.869873}, {2885.191109, 48.346397},
+ {2887.595357, 47.823505}, {2889.973474, 47.301191},
+ {2892.325490, 46.779448}, {2894.651433, 46.258270},
+ {2896.951331, 45.737651}, {2899.225212, 45.217583},
+ {2901.473103, 44.698061}, {2903.695032, 44.179077},
+ {2905.891024, 43.660626}, {2908.061107, 43.142701},
+ {2910.205307, 42.625297}, {2912.323650, 42.108406},
+ {2914.416161, 41.592022}, {2916.482865, 41.076140},
+ {2918.523787, 40.560754}, {2920.538952, 40.045857},
+ {2922.528385, 39.531443}, {2924.492109, 39.017507},
+ {2926.430147, 38.504042}, {2928.342524, 37.991043},
+ {2930.229263, 37.478504}, {2932.090386, 36.966419},
+ {2933.925916, 36.454783}, {2935.735876, 35.943589},
+ {2937.520286, 35.432833}, {2939.279170, 34.922508},
+ {2941.012548, 34.412610}, {2942.720441, 33.903132},
+ {2944.402871, 33.394069}, {2946.059858, 32.885416},
+ {2947.691423, 32.377167}, {2949.297585, 31.869318},
+ {2950.878365, 31.361863}, {2952.433781, 30.854796},
+ {2953.963854, 30.348113}, {2955.468602, 29.841809},
+ {2956.948044, 29.335879}, {2958.402199, 28.830317},
+ {2959.831085, 28.325118}, {2961.234720, 27.820279},
+ {2962.613122, 27.315793}, {2963.966308, 26.811657},
+ {2965.294296, 26.307865}, {2966.597103, 25.804413},
+ {2967.874745, 25.301296}, {2969.127241, 24.798510},
+ {2970.354605, 24.296050}, {2971.556854, 23.793912},
+ {2972.734004, 23.292091}, {2973.886071, 22.790583},
+ {2975.013070, 22.289384}, {2976.115017, 21.788489},
+ {2977.191926, 21.287895}, {2978.243814, 20.787598},
+ {2979.270693, 20.287593}, {2980.272580, 19.787876},
+ {2981.249488, 19.288444}, {2982.201431, 18.789293},
+ {2983.128424, 18.290419}, {2984.030480, 17.791818},
+ {2984.907613, 17.293487}, {2985.759835, 16.795422},
+ {2986.587162, 16.297620}, {2987.389604, 15.800078},
+ {2988.167176, 15.302792}, {2988.919890, 14.805759},
+ {2989.647758, 14.308975}, {2990.350793, 13.812439},
+ {2991.029008, 13.316146}, {2991.682414, 12.820094},
+ {2992.311023, 12.324280}, {2992.914848, 11.828702},
+ {2993.493899, 11.333357}, {2994.048189, 10.838241},
+ {2994.577729, 10.343354}, {2995.082530, 9.848691},
+ {2995.562604, 9.354252}, {2996.017961, 8.860034},
+ {2996.448613, 8.366035}, {2996.854570, 7.872253},
+ {2997.235843, 7.378685}, {2997.592444, 6.885331},
+ {2997.924382, 6.392189}, {2998.231668, 5.899256},
+ {2998.514313, 5.406532}, {2998.772326, 4.914014},
+ {2999.005719, 4.421703}, {2999.214502, 3.929596},
+ {2999.398684, 3.437693}, {2999.558276, 2.945992},
+ {2999.693288, 2.454493}, {2999.803730, 1.963195},
+ {2999.889613, 1.472097}, {2999.950945, 0.981199},
+ {2999.987737, 0.490500}, {2999.998745, 0.156943},
+ {2999.999950, 0.031348}, {2999.999998, 0.006229},
+ {3000.000000, 0.001206}, {3000.000000, 0.000201},
+ {3000.000000, 0.000000},
+ }},
+ {357,
+ {
+ {1216.978643, 250.000000}, {1216.978643, 250.000000},
+ {1226.925199, 248.731029}, {1239.322481, 247.160238},
+ {1251.641707, 245.608794}, {1263.883831, 244.076163},
+ {1276.049781, 242.561836}, {1288.140460, 241.065322},
+ {1300.156747, 239.586150}, {1312.099497, 238.123865},
+ {1323.969544, 236.678030}, {1335.767701, 235.248226},
+ {1347.494758, 233.834047}, {1359.151486, 232.435101},
+ {1370.738639, 231.051013}, {1382.256950, 229.681419},
+ {1393.707135, 228.325969}, {1405.089892, 226.984323},
+ {1416.405904, 225.656156}, {1427.655837, 224.341150},
+ {1438.840340, 223.039000}, {1449.960051, 221.749412},
+ {1461.015588, 220.472099}, {1472.007560, 219.206785},
+ {1482.936560, 217.953203}, {1493.803167, 216.711093},
+ {1504.607950, 215.480206}, {1515.351463, 214.260298},
+ {1526.034248, 213.051134}, {1536.656839, 211.852486},
+ {1547.219754, 210.664131}, {1557.723504, 209.485857},
+ {1568.168587, 208.317455}, {1578.555491, 207.158723},
+ {1588.884696, 206.009464}, {1599.156670, 204.869489},
+ {1609.371872, 203.738614}, {1619.530754, 202.616658},
+ {1629.633757, 201.503447}, {1639.681313, 200.398812},
+ {1649.673848, 199.302588}, {1659.611778, 198.214615},
+ {1669.495512, 197.134738}, {1679.325451, 196.062806},
+ {1689.101988, 194.998670}, {1698.825509, 193.942188},
+ {1708.496394, 192.893220}, {1718.115016, 191.851631},
+ {1727.681739, 190.817288}, {1737.196922, 189.790063},
+ {1746.660920, 188.769831}, {1756.074077, 187.756469},
+ {1765.436735, 186.749858}, {1774.749229, 185.749883},
+ {1784.011887, 184.756430}, {1793.225032, 183.769390},
+ {1802.388983, 182.788655}, {1811.504053, 181.814119},
+ {1820.570548, 180.845682}, {1829.588771, 179.883243},
+ {1838.559020, 178.926705}, {1847.481587, 177.975974},
+ {1856.356760, 177.030955}, {1865.184823, 176.091560},
+ {1873.966054, 175.157699}, {1882.700729, 174.229286},
+ {1891.389117, 173.306238}, {1900.031485, 172.388471},
+ {1908.628094, 171.475905}, {1917.179203, 170.568462},
+ {1925.685066, 169.666065}, {1934.145934, 168.768638},
+ {1942.562053, 167.876108}, {1950.933665, 166.988404},
+ {1959.261012, 166.105454}, {1967.544328, 165.227191},
+ {1975.783846, 164.353547}, {1983.979796, 163.484456},
+ {1992.132404, 162.619854}, {2000.241893, 161.759678},
+ {2008.308481, 160.903866}, {2016.332387, 160.052357},
+ {2024.313823, 159.205093}, {2032.253001, 158.362015},
+ {2040.150128, 157.523067}, {2048.005409, 156.688193},
+ {2055.819047, 155.857338}, {2063.591242, 155.030450},
+ {2071.322190, 154.207475}, {2079.012086, 153.388363},
+ {2086.661122, 152.573063}, {2094.269487, 151.761525},
+ {2101.837367, 150.953702}, {2109.364948, 150.149546},
+ {2116.852412, 149.349010}, {2124.299939, 148.552048},
+ {2131.707705, 147.758616}, {2139.075888, 146.968670},
+ {2146.404658, 146.182166}, {2153.694189, 145.399062},
+ {2160.944649, 144.619317}, {2168.156204, 143.842889},
+ {2175.329019, 143.069738}, {2182.463259, 142.299825},
+ {2189.559082, 141.533112}, {2196.616649, 140.769559},
+ {2203.636116, 140.009129}, {2210.617639, 139.251787},
+ {2217.561371, 138.497495}, {2224.467464, 137.746218},
+ {2231.336067, 136.997921}, {2238.167329, 136.252569},
+ {2244.961397, 135.510130}, {2251.718414, 134.770569},
+ {2258.438525, 134.033854}, {2265.121870, 133.299952},
+ {2271.768590, 132.568833}, {2278.378822, 131.840465},
+ {2284.952704, 131.114816}, {2291.490371, 130.391858},
+ {2297.991956, 129.671560}, {2304.457593, 128.953892},
+ {2310.887411, 128.238827}, {2317.281540, 127.526336},
+ {2323.640108, 126.816390}, {2329.963242, 126.108963},
+ {2336.251066, 125.404026}, {2342.503706, 124.701554},
+ {2348.721283, 124.001520}, {2354.903918, 123.303898},
+ {2361.051732, 122.608662}, {2367.164844, 121.915787},
+ {2373.243369, 121.225248}, {2379.287426, 120.537021},
+ {2385.297129, 119.851082}, {2391.272591, 119.167406},
+ {2397.213925, 118.485970}, {2403.121243, 117.806750},
+ {2408.994655, 117.129725}, {2414.834270, 116.454870},
+ {2420.640196, 115.782165}, {2426.412540, 115.111586},
+ {2432.151407, 114.443113}, {2437.856903, 113.776723},
+ {2443.529131, 113.112395}, {2449.168194, 112.450109},
+ {2454.774193, 111.789844}, {2460.347228, 111.131579},
+ {2465.887400, 110.475295}, {2471.394807, 109.820971},
+ {2476.869546, 109.168587}, {2482.311713, 108.518125},
+ {2487.721406, 107.869565}, {2493.098717, 107.222887},
+ {2498.443741, 106.578074}, {2503.756570, 105.935107},
+ {2509.037297, 105.293967}, {2514.286012, 104.654637},
+ {2519.502806, 104.017098}, {2524.687767, 103.381333},
+ {2529.840983, 102.747323}, {2534.962542, 102.115053},
+ {2540.052531, 101.484505}, {2545.111035, 100.855661},
+ {2550.138140, 100.228505}, {2555.133928, 99.603021},
+ {2560.098483, 98.979192}, {2565.031888, 98.357002},
+ {2569.934224, 97.736435}, {2574.805572, 97.117475},
+ {2579.646011, 96.500106}, {2584.455622, 95.884313},
+ {2589.234482, 95.270080}, {2593.982668, 94.657392},
+ {2598.700259, 94.046234}, {2603.387330, 93.436591},
+ {2608.043956, 92.828448}, {2612.670212, 92.221791},
+ {2617.266171, 91.616605}, {2621.831908, 91.012876},
+ {2626.367495, 90.410589}, {2630.873003, 89.809731},
+ {2635.348503, 89.210287}, {2639.794067, 88.612244},
+ {2644.209763, 88.015588}, {2648.595660, 87.420305},
+ {2652.951827, 86.826383}, {2657.278332, 86.233807},
+ {2661.575241, 85.642564}, {2665.842621, 85.052642},
+ {2670.080538, 84.464028}, {2674.289056, 83.876708},
+ {2678.468241, 83.290670}, {2682.618155, 82.705901},
+ {2686.738862, 82.122390}, {2690.830425, 81.540122},
+ {2694.892905, 80.959087}, {2698.926364, 80.379272},
+ {2702.930863, 79.800665}, {2706.906461, 79.223254},
+ {2710.853218, 78.647027}, {2714.771193, 78.071972},
+ {2718.660444, 77.498079}, {2722.521029, 76.925334},
+ {2726.353006, 76.353727}, {2730.156430, 75.783246},
+ {2733.931358, 75.213881}, {2737.677846, 74.645619},
+ {2741.395948, 74.078450}, {2745.085718, 73.512363},
+ {2748.747211, 72.947346}, {2752.380479, 72.383390},
+ {2755.985576, 71.820483}, {2759.562553, 71.258615},
+ {2763.111463, 70.697775}, {2766.632356, 70.137953},
+ {2770.125284, 69.579138}, {2773.590295, 69.021319},
+ {2777.027440, 68.464488}, {2780.436768, 67.908633},
+ {2783.818328, 67.353745}, {2787.172167, 66.799813},
+ {2790.498333, 66.246827}, {2793.796873, 65.694779},
+ {2797.067834, 65.143657}, {2800.311261, 64.593453},
+ {2803.527202, 64.044156}, {2806.715699, 63.495758},
+ {2809.876800, 62.948248}, {2813.010546, 62.401617},
+ {2816.116983, 61.855857}, {2819.196153, 61.310957},
+ {2822.248100, 60.766908}, {2825.272865, 60.223703},
+ {2828.270491, 59.681330}, {2831.241019, 59.139782},
+ {2834.184490, 58.599049}, {2837.100944, 58.059123},
+ {2839.990422, 57.519995}, {2842.852963, 56.981656},
+ {2845.688607, 56.444098}, {2848.497392, 55.907311},
+ {2851.279357, 55.371288}, {2854.034540, 54.836020},
+ {2856.762978, 54.301498}, {2859.464708, 53.767715},
+ {2862.139768, 53.234662}, {2864.788192, 52.702330},
+ {2867.410019, 52.170712}, {2870.005281, 51.639800},
+ {2872.574016, 51.109585}, {2875.116257, 50.580059},
+ {2877.632039, 50.051216}, {2880.121395, 49.523046},
+ {2882.584360, 48.995542}, {2885.020966, 48.468697},
+ {2887.431246, 47.942502}, {2889.815232, 47.416950},
+ {2892.172957, 46.892033}, {2894.504451, 46.367745},
+ {2896.809747, 45.844077}, {2899.088874, 45.321022},
+ {2901.341864, 44.798573}, {2903.568747, 44.276723},
+ {2905.769551, 43.755464}, {2907.944308, 43.234789},
+ {2910.093045, 42.714691}, {2912.215791, 42.195163},
+ {2914.312575, 41.676198}, {2916.383425, 41.157790},
+ {2918.428368, 40.639930}, {2920.447431, 40.122613},
+ {2922.440643, 39.605832}, {2924.408028, 39.089579},
+ {2926.349613, 38.573848}, {2928.265426, 38.058634},
+ {2930.155490, 37.543928}, {2932.019831, 37.029725},
+ {2933.858474, 36.516018}, {2935.671445, 36.002800},
+ {2937.458767, 35.490067}, {2939.220463, 34.977810},
+ {2940.956559, 34.466024}, {2942.667077, 33.954703},
+ {2944.352041, 33.443840}, {2946.011473, 32.933430},
+ {2947.645395, 32.423467}, {2949.253831, 31.913944},
+ {2950.836801, 31.404856}, {2952.394327, 30.896198},
+ {2953.926431, 30.387962}, {2955.433134, 29.880143},
+ {2956.914456, 29.372737}, {2958.370417, 28.865737},
+ {2959.801039, 28.359137}, {2961.206341, 27.852933},
+ {2962.586342, 27.347118}, {2963.941062, 26.841688},
+ {2965.270521, 26.336637}, {2966.574735, 25.831960},
+ {2967.853726, 25.327652}, {2969.107510, 24.823708},
+ {2970.336105, 24.320122}, {2971.539531, 23.816890},
+ {2972.717803, 23.314007}, {2973.870940, 22.811468},
+ {2974.998958, 22.309268}, {2976.101875, 21.807403},
+ {2977.179707, 21.305867}, {2978.232470, 20.804658},
+ {2979.260181, 20.303769}, {2980.262855, 19.803196},
+ {2981.240508, 19.302936}, {2982.193156, 18.802984},
+ {2983.120814, 18.303335}, {2984.023497, 17.803987},
+ {2984.901220, 17.304934}, {2985.753998, 16.806173},
+ {2986.581845, 16.307700}, {2987.384775, 15.809511},
+ {2988.162803, 15.311602}, {2988.915942, 14.813971},
+ {2989.644207, 14.316613}, {2990.347610, 13.819525},
+ {2991.026166, 13.322704}, {2991.679887, 12.826146},
+ {2992.308787, 12.329849}, {2992.912879, 11.833809},
+ {2993.492174, 11.338024}, {2994.046687, 10.842490},
+ {2994.576430, 10.347205}, {2995.081414, 9.852166},
+ {2995.561652, 9.357371}, {2996.017157, 8.862818},
+ {2996.447940, 8.368503}, {2996.854013, 7.874425},
+ {2997.235388, 7.380582}, {2997.592077, 6.886971},
+ {2997.924091, 6.393591}, {2998.231442, 5.900440},
+ {2998.514141, 5.407517}, {2998.772199, 4.914819},
+ {2999.005628, 4.422346}, {2999.214439, 3.930095},
+ {2999.398643, 3.438067}, {2999.558252, 2.946259},
+ {2999.693275, 2.454671}, {2999.803724, 1.963301},
+ {2999.889610, 1.472150}, {2999.950945, 0.981217},
+ {2999.987737, 0.490500}, {2999.998745, 0.156943},
+ {2999.999950, 0.031348}, {2999.999998, 0.006229},
+ {3000.000000, 0.001206}, {3000.000000, 0.000201},
+ {3000.000000, 0.000000},
+ }},
+ {348,
+ {
+ {1290.095815, 250.000000}, {1290.095815, 250.000000},
+ {1299.209094, 248.731151}, {1311.602879, 247.020240},
+ {1323.911725, 245.333604}, {1336.136827, 243.670496},
+ {1348.279345, 242.030200}, {1360.340401, 240.412031},
+ {1372.321085, 238.815334}, {1384.222455, 237.239479},
+ {1396.045539, 235.683863}, {1407.791333, 234.147907},
+ {1419.460807, 232.631056}, {1431.054903, 231.132777},
+ {1442.574536, 229.652556}, {1454.020597, 228.189902},
+ {1465.393954, 226.744341}, {1476.695447, 225.315416},
+ {1487.925900, 223.902690}, {1499.086111, 222.505739},
+ {1510.176858, 221.124157}, {1521.198901, 219.757553},
+ {1532.152979, 218.405548}, {1543.039812, 217.067777},
+ {1553.860103, 215.743891}, {1564.614539, 214.433548},
+ {1575.303789, 213.136423}, {1585.928504, 211.852200},
+ {1596.489324, 210.580572}, {1606.986869, 209.321246},
+ {1617.421748, 208.073935}, {1627.794556, 206.838366},
+ {1638.105872, 205.614270}, {1648.356263, 204.401391},
+ {1658.546285, 203.199478}, {1668.676479, 202.008292},
+ {1678.747377, 200.827597}, {1688.759496, 199.657167},
+ {1698.713344, 198.496784}, {1708.609420, 197.346234},
+ {1718.448209, 196.205312}, {1728.230187, 195.073818},
+ {1737.955821, 193.951559}, {1747.625569, 192.838346},
+ {1757.239878, 191.733998}, {1766.799186, 190.638337},
+ {1776.303924, 189.551193}, {1785.754514, 188.472398},
+ {1795.151369, 187.401790}, {1804.494894, 186.339213},
+ {1813.785487, 185.284513}, {1823.023538, 184.237543},
+ {1832.209431, 183.198157}, {1841.343540, 182.166216},
+ {1850.426235, 181.141583}, {1859.457878, 180.124125},
+ {1868.438824, 179.113714}, {1877.369422, 178.110223},
+ {1886.250016, 177.113530}, {1895.080942, 176.123516},
+ {1903.862532, 175.140066}, {1912.595110, 174.163066},
+ {1921.278997, 173.192406}, {1929.914506, 172.227979},
+ {1938.501948, 171.269681}, {1947.041625, 170.317409},
+ {1955.533837, 169.371065}, {1963.978877, 168.430551},
+ {1972.377035, 167.495774}, {1980.728596, 166.566641},
+ {1989.033838, 165.643062}, {1997.293039, 164.724949},
+ {2005.506468, 163.812217}, {2013.674393, 162.904782},
+ {2021.797076, 162.002563}, {2029.874778, 161.105480},
+ {2037.907751, 160.213454}, {2045.896247, 159.326410},
+ {2053.840515, 158.444274}, {2061.740796, 157.566973},
+ {2069.597331, 156.694435}, {2077.410357, 155.826591},
+ {2085.180106, 154.963374}, {2092.906808, 154.104716},
+ {2100.590690, 153.250554}, {2108.231974, 152.400822},
+ {2115.830881, 151.555460}, {2123.387628, 150.714405},
+ {2130.902428, 149.877599}, {2138.375492, 149.044982},
+ {2145.807029, 148.216499}, {2153.197244, 147.392092},
+ {2160.546339, 146.571707}, {2167.854514, 145.755290},
+ {2175.121966, 144.942788}, {2182.348889, 144.134150},
+ {2189.535476, 143.329324}, {2196.681916, 142.528263},
+ {2203.788395, 141.730915}, {2210.855099, 140.937235},
+ {2217.882209, 140.147175}, {2224.869906, 139.360689},
+ {2231.818366, 138.577731}, {2238.727766, 137.798259},
+ {2245.598278, 137.022227}, {2252.430074, 136.249594},
+ {2259.223322, 135.480318}, {2265.978189, 134.714357},
+ {2272.694839, 133.951672}, {2279.373437, 133.192221},
+ {2286.014141, 132.435968}, {2292.617112, 131.682872},
+ {2299.182507, 130.932896}, {2305.710479, 130.186004},
+ {2312.201183, 129.442159}, {2318.654770, 128.701325},
+ {2325.071390, 127.963467}, {2331.451191, 127.228551},
+ {2337.794318, 126.496541}, {2344.100917, 125.767406},
+ {2350.371130, 125.041111}, {2356.605098, 124.317625},
+ {2362.802961, 123.596915}, {2368.964858, 122.878950},
+ {2375.090924, 122.163698}, {2381.181295, 121.451131},
+ {2387.236104, 120.741217}, {2393.255482, 120.033927},
+ {2399.239561, 119.329232}, {2405.188470, 118.627103},
+ {2411.102335, 117.927512}, {2416.981283, 117.230430},
+ {2422.825440, 116.535832}, {2428.634928, 115.843689},
+ {2434.409870, 115.153974}, {2440.150386, 114.466663},
+ {2445.856595, 113.781728}, {2451.528617, 113.099143},
+ {2457.166568, 112.418885}, {2462.770563, 111.740928},
+ {2468.340717, 111.065246}, {2473.877144, 110.391817},
+ {2479.379955, 109.720616}, {2484.849261, 109.051620},
+ {2490.285171, 108.384805}, {2495.687795, 107.720149},
+ {2501.057240, 107.057628}, {2506.393611, 106.397220},
+ {2511.697014, 105.738903}, {2516.967553, 105.082656},
+ {2522.205331, 104.428456}, {2527.410449, 103.776283},
+ {2532.583009, 103.126115}, {2537.723110, 102.477932},
+ {2542.830851, 101.831713}, {2547.906330, 101.187437},
+ {2552.949643, 100.545085}, {2557.960886, 99.904636},
+ {2562.940154, 99.266072}, {2567.887540, 98.629373},
+ {2572.803137, 97.994519}, {2577.687038, 97.361492},
+ {2582.539332, 96.730274}, {2587.360110, 96.100844},
+ {2592.149461, 95.473186}, {2596.907472, 94.847280},
+ {2601.634232, 94.223110}, {2606.329826, 93.600657},
+ {2610.994340, 92.979904}, {2615.627859, 92.360834},
+ {2620.230465, 91.743429}, {2624.802243, 91.127672},
+ {2629.343273, 90.513547}, {2633.853638, 89.901037},
+ {2638.333417, 89.290126}, {2642.782690, 88.680796},
+ {2647.201536, 88.073033}, {2651.590032, 87.466820},
+ {2655.948256, 86.862141}, {2660.276284, 86.258980},
+ {2664.574192, 85.657323}, {2668.842054, 85.057154},
+ {2673.079944, 84.458457}, {2677.287936, 83.861218},
+ {2681.466102, 83.265421}, {2685.614514, 82.671053},
+ {2689.733242, 82.078098}, {2693.822358, 81.486541},
+ {2697.881931, 80.896369}, {2701.912030, 80.307568},
+ {2705.912722, 79.720122}, {2709.884075, 79.134019},
+ {2713.826157, 78.549244}, {2717.739033, 77.965784},
+ {2721.622768, 77.383625}, {2725.477427, 76.802754},
+ {2729.303075, 76.223157}, {2733.099775, 75.644822},
+ {2736.867589, 75.067734}, {2740.606579, 74.491882},
+ {2744.316807, 73.917252}, {2747.998334, 73.343832},
+ {2751.651220, 72.771608}, {2755.275525, 72.200569},
+ {2758.871307, 71.630701}, {2762.438624, 71.061994},
+ {2765.977535, 70.494433}, {2769.488096, 69.928008},
+ {2772.970363, 69.362707}, {2776.424394, 68.798516},
+ {2779.850243, 68.235426}, {2783.247964, 67.673423},
+ {2786.617612, 67.112497}, {2789.959240, 66.552636},
+ {2793.272902, 65.993828}, {2796.558649, 65.436062},
+ {2799.816534, 64.879327}, {2803.046607, 64.323612},
+ {2806.248920, 63.768905}, {2809.423523, 63.215197},
+ {2812.570464, 62.662475}, {2815.689794, 62.110729},
+ {2818.781561, 61.559949}, {2821.845813, 61.010124},
+ {2824.882597, 60.461243}, {2827.891961, 59.913297},
+ {2830.873950, 59.366274}, {2833.828611, 58.820164},
+ {2836.755989, 58.274957}, {2839.656129, 57.730644},
+ {2842.529076, 57.187214}, {2845.374872, 56.644657},
+ {2848.193563, 56.102963}, {2850.985190, 55.562123},
+ {2853.749796, 55.022126}, {2856.487424, 54.482964},
+ {2859.198113, 53.944627}, {2861.881907, 53.407104},
+ {2864.538844, 52.870388}, {2867.168965, 52.334468},
+ {2869.772310, 51.799335}, {2872.348918, 51.264981},
+ {2874.898828, 50.731395}, {2877.422077, 50.198569},
+ {2879.918703, 49.666495}, {2882.388745, 49.135162},
+ {2884.832238, 48.604563}, {2887.249219, 48.074688},
+ {2889.639725, 47.545530}, {2892.003790, 47.017078},
+ {2894.341450, 46.489326}, {2896.652740, 45.962263},
+ {2898.937693, 45.435883}, {2901.196345, 44.910176},
+ {2903.428728, 44.385134}, {2905.634875, 43.860750},
+ {2907.814819, 43.337014}, {2909.968592, 42.813920},
+ {2912.096227, 42.291458}, {2914.197754, 41.769622},
+ {2916.273204, 41.248402}, {2918.322609, 40.727792},
+ {2920.345998, 40.207784}, {2922.343402, 39.688369},
+ {2924.314850, 39.169541}, {2926.260371, 38.651291},
+ {2928.179993, 38.133613}, {2930.073746, 37.616499},
+ {2931.941657, 37.099942}, {2933.783754, 36.583934},
+ {2935.600064, 36.068467}, {2937.390614, 35.553536},
+ {2939.155431, 35.039133}, {2940.894541, 34.525251},
+ {2942.607969, 34.011882}, {2944.295742, 33.499021},
+ {2945.957884, 32.986660}, {2947.594420, 32.474792},
+ {2949.205375, 31.963412}, {2950.790773, 31.452511},
+ {2952.350638, 30.942084}, {2953.884993, 30.432124},
+ {2955.393862, 29.922625}, {2956.877267, 29.413581},
+ {2958.335231, 28.904984}, {2959.767777, 28.396830},
+ {2961.174925, 27.889111}, {2962.556698, 27.381822},
+ {2963.913118, 26.874957}, {2965.244204, 26.368509},
+ {2966.549979, 25.862473}, {2967.830462, 25.356844},
+ {2969.085673, 24.851615}, {2970.315633, 24.346781},
+ {2971.520361, 23.842337}, {2972.699877, 23.338276},
+ {2973.854198, 22.834593}, {2974.983345, 22.331284},
+ {2976.087336, 21.828342}, {2977.166189, 21.325764},
+ {2978.219921, 20.823543}, {2979.248552, 20.321674},
+ {2980.252097, 19.820153}, {2981.230576, 19.318976},
+ {2982.184003, 18.818136}, {2983.112397, 18.317630},
+ {2984.015774, 17.817453}, {2984.894151, 17.317600},
+ {2985.747543, 16.818068}, {2986.575965, 16.318851},
+ {2987.379435, 15.819946}, {2988.157968, 15.321349},
+ {2988.911578, 14.823055}, {2989.640281, 14.325061},
+ {2990.344091, 13.827362}, {2991.023024, 13.329956},
+ {2991.677094, 12.832839}, {2992.306315, 12.336007},
+ {2992.910702, 11.839456}, {2993.490268, 11.343184},
+ {2994.045027, 10.847187}, {2994.574993, 10.351463},
+ {2995.080180, 9.856007}, {2995.560601, 9.360819},
+ {2996.016269, 8.865894}, {2996.447197, 8.371230},
+ {2996.853398, 7.876826}, {2997.234886, 7.382677},
+ {2997.591672, 6.888783}, {2997.923770, 6.395141},
+ {2998.231192, 5.901749}, {2998.513951, 5.408605},
+ {2998.772059, 4.915708}, {2999.005528, 4.423055},
+ {2999.214371, 3.930646}, {2999.398599, 3.438479},
+ {2999.558225, 2.946553}, {2999.693260, 2.454866},
+ {2999.803717, 1.963419}, {2999.889608, 1.472209},
+ {2999.950944, 0.981236}, {2999.987737, 0.490500},
+ {2999.998745, 0.156943}, {2999.999950, 0.031348},
+ {2999.999998, 0.006229}, {3000.000000, 0.001206},
+ {3000.000000, 0.000201}, {3000.000000, 0.000000},
+ }},
+ {340,
+ {
+ {1361.522768, 250.000000}, {1361.522768, 250.000000},
+ {1361.522768, 250.000000}, {1370.491594, 248.638116},
+ {1382.876958, 246.776445}, {1395.169992, 244.944924},
+ {1407.372179, 243.142520}, {1419.484948, 241.368247},
+ {1431.509683, 239.621165}, {1443.447722, 237.900376},
+ {1455.300357, 236.205024}, {1467.068839, 234.534292},
+ {1478.754382, 232.887397}, {1490.358156, 231.263594},
+ {1501.881300, 229.662166}, {1513.324915, 228.082430},
+ {1524.690069, 226.523731}, {1535.977799, 224.985442},
+ {1547.189109, 223.466960}, {1558.324976, 221.967711},
+ {1569.386347, 220.487139}, {1580.374143, 219.024716},
+ {1591.289259, 217.579930}, {1602.132565, 216.152294},
+ {1612.904906, 214.741336}, {1623.607104, 213.346605},
+ {1634.239961, 211.967666}, {1644.804255, 210.604102},
+ {1655.300746, 209.255509}, {1665.730171, 207.921501},
+ {1676.093251, 206.601704}, {1686.390688, 205.295760},
+ {1696.623165, 204.003322}, {1706.791349, 202.724057},
+ {1716.895891, 201.457641}, {1726.937427, 200.203766},
+ {1736.916574, 198.962131}, {1746.833939, 197.732447},
+ {1756.690111, 196.514435}, {1766.485667, 195.307825},
+ {1776.221172, 194.112355}, {1785.897175, 192.927775},
+ {1795.514215, 191.753840}, {1805.072819, 190.590316},
+ {1814.573501, 189.436973}, {1824.016765, 188.293591},
+ {1833.403104, 187.159957}, {1842.733000, 186.035864},
+ {1852.006924, 184.921112}, {1861.225340, 183.815507},
+ {1870.388699, 182.718860}, {1879.497445, 181.630990},
+ {1888.552013, 180.551719}, {1897.552828, 179.480877},
+ {1906.500307, 178.418297}, {1915.394860, 177.363817},
+ {1924.236887, 176.317281}, {1933.026783, 175.278538},
+ {1941.764932, 174.247439}, {1950.451714, 173.223840},
+ {1959.087500, 172.207603}, {1967.672655, 171.198592},
+ {1976.207537, 170.196675}, {1984.692497, 169.201725},
+ {1993.127880, 168.213616}, {2001.514026, 167.232228},
+ {2009.851268, 166.257442}, {2018.139933, 165.289144},
+ {2026.380342, 164.327223}, {2034.572812, 163.371569},
+ {2042.717653, 162.422076}, {2050.815171, 161.478642},
+ {2058.865666, 160.541165}, {2066.869434, 159.609548},
+ {2074.826765, 158.683695}, {2082.737945, 157.763513},
+ {2090.603256, 156.848911}, {2098.422973, 155.939800},
+ {2106.197371, 155.036095}, {2113.926716, 154.137709},
+ {2121.611273, 153.244562}, {2129.251301, 152.356573},
+ {2136.847057, 151.473663}, {2144.398792, 150.595755},
+ {2151.906756, 149.722775}, {2159.371191, 148.854648},
+ {2166.792340, 147.991304}, {2174.170439, 147.132673},
+ {2181.505723, 146.278686}, {2188.798422, 145.429276},
+ {2196.048764, 144.584378}, {2203.256971, 143.743927},
+ {2210.423266, 142.907861}, {2217.547866, 142.076119},
+ {2224.630985, 141.248640}, {2231.672835, 140.425367},
+ {2238.673625, 139.606240}, {2245.633561, 138.791204},
+ {2252.552846, 137.980204}, {2259.431681, 137.173185},
+ {2266.270263, 136.370094}, {2273.068787, 135.570879},
+ {2279.827447, 134.775490}, {2286.546431, 133.983876},
+ {2293.225927, 133.195988}, {2299.866121, 132.411779},
+ {2306.467196, 131.631200}, {2313.029331, 130.854206},
+ {2319.552705, 130.080750}, {2326.037493, 129.310789},
+ {2332.483870, 128.544279}, {2338.892007, 127.781177},
+ {2345.262072, 127.021439}, {2351.594234, 126.265026},
+ {2357.888657, 125.511895}, {2364.145504, 124.762007},
+ {2370.364937, 124.015323}, {2376.547116, 123.271804},
+ {2382.692196, 122.531411}, {2388.800334, 121.794108},
+ {2394.871683, 121.059857}, {2400.906395, 120.328622},
+ {2406.904620, 119.600368}, {2412.866506, 118.875060},
+ {2418.792199, 118.152662}, {2424.681844, 117.433142},
+ {2430.535584, 116.716466}, {2436.353560, 116.002600},
+ {2442.135913, 115.291513}, {2447.882780, 114.583172},
+ {2453.594298, 113.877546}, {2459.270602, 113.174605},
+ {2464.911825, 112.474318}, {2470.518100, 111.776654},
+ {2476.089555, 111.081584}, {2481.626322, 110.389079},
+ {2487.128527, 109.699110}, {2492.596296, 109.011649},
+ {2498.029754, 108.326668}, {2503.429024, 107.644139},
+ {2508.794228, 106.964036}, {2514.125487, 106.286330},
+ {2519.422921, 105.610997}, {2524.686646, 104.938009},
+ {2529.916779, 104.267341}, {2535.113437, 103.598968},
+ {2540.276733, 102.932865}, {2545.406780, 102.269006},
+ {2550.503689, 101.607367}, {2555.567571, 100.947925},
+ {2560.598536, 100.290656}, {2565.596691, 99.635535},
+ {2570.562143, 98.982540}, {2575.494997, 98.331648},
+ {2580.395359, 97.682836}, {2585.263332, 97.036082},
+ {2590.099018, 96.391363}, {2594.902519, 95.748659},
+ {2599.673934, 95.107946}, {2604.413363, 94.469205},
+ {2609.120903, 93.832413}, {2613.796652, 93.197550},
+ {2618.440706, 92.564596}, {2623.053159, 91.933529},
+ {2627.634106, 91.304331}, {2632.183638, 90.676981},
+ {2636.701849, 90.051459}, {2641.188830, 89.427745},
+ {2645.644669, 88.805822}, {2650.069456, 88.185669},
+ {2654.463279, 87.567267}, {2658.826226, 86.950599},
+ {2663.158382, 86.335646}, {2667.459833, 85.722388},
+ {2671.730663, 85.110810}, {2675.970955, 84.500891},
+ {2680.180793, 83.892616}, {2684.360258, 83.285966},
+ {2688.509430, 82.680924}, {2692.628390, 82.077473},
+ {2696.717217, 81.475596}, {2700.775988, 80.875276},
+ {2704.804783, 80.276497}, {2708.803676, 79.679242},
+ {2712.772745, 79.083494}, {2716.712063, 78.489239},
+ {2720.621705, 77.896459}, {2724.501745, 77.305139},
+ {2728.352255, 76.715263}, {2732.173307, 76.126816},
+ {2735.964972, 75.539783}, {2739.727321, 74.954148},
+ {2743.460422, 74.369895}, {2747.164344, 73.787011},
+ {2750.839157, 73.205481}, {2754.484926, 72.625289},
+ {2758.101719, 72.046421}, {2761.689601, 71.468863},
+ {2765.248637, 70.892601}, {2768.778893, 70.317620},
+ {2772.280431, 69.743907}, {2775.753315, 69.171447},
+ {2779.197607, 68.600227}, {2782.613368, 68.030234},
+ {2786.000660, 67.461453}, {2789.359544, 66.893872},
+ {2792.690077, 66.327477}, {2795.992321, 65.762255},
+ {2799.266332, 65.198193}, {2802.512168, 64.635278},
+ {2805.729888, 64.073497}, {2808.919546, 63.512838},
+ {2812.081199, 62.953289}, {2815.214902, 62.394836},
+ {2818.320710, 61.837467}, {2821.398676, 61.281170},
+ {2824.448854, 60.725934}, {2827.471296, 60.171745},
+ {2830.466054, 59.618592}, {2833.433180, 59.066463},
+ {2836.372726, 58.515347}, {2839.284740, 57.965232},
+ {2842.169273, 57.416106}, {2845.026375, 56.867958},
+ {2847.856093, 56.320776}, {2850.658477, 55.774550},
+ {2853.433572, 55.229268}, {2856.181427, 54.684919},
+ {2858.902087, 54.141492}, {2861.595599, 53.598977},
+ {2864.262007, 53.057362}, {2866.901357, 52.516637},
+ {2869.513693, 51.976791}, {2872.099058, 51.437815},
+ {2874.657496, 50.899696}, {2877.189049, 50.362426},
+ {2879.693759, 49.825994}, {2882.171669, 49.290389},
+ {2884.622819, 48.755602}, {2887.047249, 48.221623},
+ {2889.445001, 47.688442}, {2891.816113, 47.156048},
+ {2894.160625, 46.624433}, {2896.478576, 46.093587},
+ {2898.770003, 45.563500}, {2901.034944, 45.034162},
+ {2903.273438, 44.505564}, {2905.485519, 43.977698},
+ {2907.671225, 43.450553}, {2909.830592, 42.924120},
+ {2911.963655, 42.398392}, {2914.070449, 41.873357},
+ {2916.151008, 41.349008}, {2918.205367, 40.825336},
+ {2920.233558, 40.302332}, {2922.235616, 39.779987},
+ {2924.211573, 39.258292}, {2926.161461, 38.737240},
+ {2928.085313, 38.216821}, {2929.983159, 37.697028},
+ {2931.855031, 37.177852}, {2933.700960, 36.659285},
+ {2935.520975, 36.141318}, {2937.315106, 35.623944},
+ {2939.083384, 35.107154}, {2940.825836, 34.590941},
+ {2942.542492, 34.075297}, {2944.233380, 33.560215},
+ {2945.898527, 33.045685}, {2947.537962, 32.531702},
+ {2949.151711, 32.018257}, {2950.739801, 31.505342},
+ {2952.302258, 30.992952}, {2953.839109, 30.481077},
+ {2955.350379, 29.969712}, {2956.836093, 29.458848},
+ {2958.296276, 28.948479}, {2959.730953, 28.438598},
+ {2961.140148, 27.929199}, {2962.523885, 27.420273},
+ {2963.882187, 26.911815}, {2965.215078, 26.403817},
+ {2966.522580, 25.896274}, {2967.804716, 25.389179},
+ {2969.061509, 24.882525}, {2970.292980, 24.376306},
+ {2971.499150, 23.870517}, {2972.680042, 23.365149},
+ {2973.835676, 22.860199}, {2974.966072, 22.355659},
+ {2976.071252, 21.851525}, {2977.151234, 21.347789},
+ {2978.206040, 20.844447}, {2979.235689, 20.341493},
+ {2980.240199, 19.838921}, {2981.219590, 19.336727},
+ {2982.173881, 18.834904}, {2983.103090, 18.333447},
+ {2984.007235, 17.832352}, {2984.886334, 17.331614},
+ {2985.740405, 16.831227}, {2986.569466, 16.331187},
+ {2987.373532, 15.831489}, {2988.152623, 15.332129},
+ {2988.906754, 14.833102}, {2989.635941, 14.334403},
+ {2990.340202, 13.836029}, {2991.019552, 13.337976},
+ {2991.674008, 12.840239}, {2992.303584, 12.342814},
+ {2992.908297, 11.845699}, {2993.488161, 11.348888},
+ {2994.043193, 10.852379}, {2994.573407, 10.356168},
+ {2995.078817, 9.860252}, {2995.559439, 9.364628},
+ {2996.015287, 8.869293}, {2996.446376, 8.374244},
+ {2996.852719, 7.879478}, {2997.234331, 7.384992},
+ {2997.591225, 6.890784}, {2997.923416, 6.396852},
+ {2998.230917, 5.903194}, {2998.513742, 5.409807},
+ {2998.771904, 4.916689}, {2999.005418, 4.423839},
+ {2999.214295, 3.931254}, {2999.398550, 3.438935},
+ {2999.558195, 2.946878}, {2999.693244, 2.455083},
+ {2999.803710, 1.963548}, {2999.889605, 1.472274},
+ {2999.950944, 0.981258}, {2999.987737, 0.490500},
+ {2999.998745, 0.156943}, {2999.999950, 0.031348},
+ {2999.999998, 0.006229}, {3000.000000, 0.001206},
+ {3000.000000, 0.000201}, {3000.000000, 0.000000},
+ }},
+ {330,
+ {
+ {1431.374621, 250.000000}, {1431.374621, 250.000000},
+ {1440.738517, 248.450148}, {1453.110427, 246.426266},
+ {1465.382072, 244.439519}, {1477.555272, 242.488495},
+ {1489.631781, 240.571853}, {1501.613285, 238.688321},
+ {1513.501410, 236.836690}, {1525.297723, 235.015811},
+ {1537.003733, 233.224592}, {1548.620898, 231.461993},
+ {1560.150623, 229.727025}, {1571.594267, 228.018745},
+ {1582.953142, 226.336255}, {1594.228516, 224.678699},
+ {1605.421615, 223.045258}, {1616.533625, 221.435153},
+ {1627.565695, 219.847640}, {1638.518936, 218.282006},
+ {1649.394426, 216.737571}, {1660.193207, 215.213685},
+ {1670.916292, 213.709724}, {1681.564663, 212.225094},
+ {1692.139271, 210.759222}, {1702.641040, 209.311563},
+ {1713.070869, 207.881592}, {1723.429629, 206.468807},
+ {1733.718168, 205.072724}, {1743.937308, 203.692882},
+ {1754.087851, 202.328836}, {1764.170576, 200.980158},
+ {1774.186240, 199.646439}, {1784.135584, 198.327284},
+ {1794.019323, 197.022314}, {1803.838160, 195.731163},
+ {1813.592777, 194.453481}, {1823.283837, 193.188929},
+ {1832.911990, 191.937182}, {1842.477867, 190.697925},
+ {1851.982087, 189.470855}, {1861.425250, 188.255682},
+ {1870.807945, 187.052124}, {1880.130746, 185.859908},
+ {1889.394213, 184.678773}, {1898.598894, 183.508467},
+ {1907.745324, 182.348743}, {1916.834027, 181.199366},
+ {1925.865514, 180.060108}, {1934.840285, 178.930747},
+ {1943.758831, 177.811071}, {1952.621629, 176.700872},
+ {1961.429150, 175.599951}, {1970.181852, 174.508114},
+ {1978.880184, 173.425174}, {1987.524587, 172.350948},
+ {1996.115492, 171.285262}, {2004.653322, 170.227943},
+ {2013.138491, 169.178828}, {2021.571406, 168.137756},
+ {2029.952464, 167.104570}, {2038.282056, 166.079121},
+ {2046.560566, 165.061261}, {2054.788369, 164.050848},
+ {2062.965834, 163.047744}, {2071.093323, 162.051815},
+ {2079.171191, 161.062930}, {2087.199788, 160.080962},
+ {2095.179457, 159.105788}, {2103.110534, 158.137288},
+ {2110.993350, 157.175345}, {2118.828230, 156.219846},
+ {2126.615493, 155.270681}, {2134.355453, 154.327741},
+ {2142.048420, 153.390922}, {2149.694696, 152.460122},
+ {2157.294580, 151.535241}, {2164.848366, 150.616183},
+ {2172.356342, 149.702853}, {2179.818792, 148.795158},
+ {2187.235996, 147.893010}, {2194.608229, 146.996320},
+ {2201.935762, 146.105003}, {2209.218862, 145.218975},
+ {2216.457790, 144.338155}, {2223.652806, 143.462464},
+ {2230.804163, 142.591823}, {2237.912112, 141.726158},
+ {2244.976901, 140.865392}, {2251.998772, 140.009455},
+ {2258.977966, 139.158275}, {2265.914717, 138.311783},
+ {2272.809259, 137.469911}, {2279.661822, 136.632593},
+ {2286.472631, 135.799764}, {2293.241909, 134.971361},
+ {2299.969876, 134.147320}, {2306.656749, 133.327583},
+ {2313.302740, 132.512088}, {2319.908062, 131.700778},
+ {2326.472921, 130.893595}, {2332.997523, 130.090484},
+ {2339.482070, 129.291389}, {2345.926761, 128.496257},
+ {2352.331794, 127.705035}, {2358.697361, 126.917671},
+ {2365.023656, 126.134115}, {2371.310867, 125.354316},
+ {2377.559180, 124.578225}, {2383.768781, 123.805796},
+ {2389.939850, 123.036980}, {2396.072568, 122.271732},
+ {2402.167111, 121.510006}, {2408.223655, 120.751757},
+ {2414.242373, 119.996942}, {2420.223434, 119.245518},
+ {2426.167009, 118.497443}, {2432.073261, 117.752674},
+ {2437.942358, 117.011172}, {2443.774459, 116.272895},
+ {2449.569727, 115.537805}, {2455.328318, 114.805863},
+ {2461.050391, 114.077030}, {2466.736098, 113.351269},
+ {2472.385594, 112.628543}, {2477.999028, 111.908815},
+ {2483.576549, 111.192050}, {2489.118306, 110.478213},
+ {2494.624443, 109.767269}, {2500.095104, 109.059183},
+ {2505.530432, 108.353922}, {2510.930566, 107.651452},
+ {2516.295646, 106.951742}, {2521.625808, 106.254759},
+ {2526.921189, 105.560471}, {2532.181922, 104.868847},
+ {2537.408140, 104.179857}, {2542.599973, 103.493469},
+ {2547.757551, 102.809654}, {2552.881002, 102.128382},
+ {2557.970452, 101.449625}, {2563.026027, 100.773353},
+ {2568.047849, 100.099539}, {2573.036041, 99.428153},
+ {2577.990724, 98.759170}, {2582.912017, 98.092561},
+ {2587.800039, 97.428300}, {2592.654905, 96.766360},
+ {2597.476732, 96.106715}, {2602.265634, 95.449339},
+ {2607.021722, 94.794207}, {2611.745110, 94.141293},
+ {2616.435907, 93.490574}, {2621.094221, 92.842023},
+ {2625.720162, 92.195617}, {2630.313836, 91.551333},
+ {2634.875348, 90.909145}, {2639.404803, 90.269032},
+ {2643.902303, 89.630970}, {2648.367950, 88.994935},
+ {2652.801846, 88.360906}, {2657.204090, 87.728861},
+ {2661.574781, 87.098776}, {2665.914017, 86.470631},
+ {2670.221892, 85.844404}, {2674.498504, 85.220073},
+ {2678.743947, 84.597618}, {2682.958312, 83.977018},
+ {2687.141694, 83.358251}, {2691.294183, 82.741299},
+ {2695.415869, 82.126141}, {2699.506841, 81.512756},
+ {2703.567188, 80.901125}, {2707.596997, 80.291229},
+ {2711.596354, 79.683048}, {2715.565344, 79.076563},
+ {2719.504052, 78.471756}, {2723.412562, 77.868608},
+ {2727.290954, 77.267100}, {2731.139312, 76.667214},
+ {2734.957716, 76.068932}, {2738.746245, 75.472236},
+ {2742.504978, 74.877108}, {2746.233994, 74.283531},
+ {2749.933370, 73.691487}, {2753.603181, 73.100959},
+ {2757.243503, 72.511931}, {2760.854411, 71.924385},
+ {2764.435978, 71.338304}, {2767.988278, 70.753673},
+ {2771.511382, 70.170475}, {2775.005361, 69.588693},
+ {2778.470286, 69.008311}, {2781.906226, 68.429314},
+ {2785.313251, 67.851687}, {2788.691429, 67.275412},
+ {2792.040826, 66.700476}, {2795.361510, 66.126862},
+ {2798.653545, 65.554555}, {2801.916997, 64.983541},
+ {2805.151931, 64.413805}, {2808.358409, 63.845332},
+ {2811.536495, 63.278107}, {2814.686251, 62.712116},
+ {2817.807737, 62.147345}, {2820.901016, 61.583779},
+ {2823.966145, 61.021404}, {2827.003185, 60.460207},
+ {2830.012195, 59.900174}, {2832.993232, 59.341291},
+ {2835.946352, 58.783544}, {2838.871614, 58.226921},
+ {2841.769072, 57.671407}, {2844.638782, 57.116990},
+ {2847.480798, 56.563657}, {2850.295175, 56.011395},
+ {2853.081964, 55.460190}, {2855.841220, 54.910030},
+ {2858.572993, 54.360904}, {2861.277336, 53.812797},
+ {2863.954298, 53.265698}, {2866.603930, 52.719595},
+ {2869.226282, 52.174475}, {2871.821402, 51.630326},
+ {2874.389339, 51.087137}, {2876.930140, 50.544896},
+ {2879.443852, 50.003590}, {2881.930522, 49.463209},
+ {2884.390195, 48.923741}, {2886.822918, 48.385174},
+ {2889.228735, 47.847498}, {2891.607690, 47.310700},
+ {2893.959827, 46.774770}, {2896.285188, 46.239697},
+ {2898.583818, 45.705471}, {2900.855756, 45.172079},
+ {2903.101046, 44.639512}, {2905.319728, 44.107759},
+ {2907.511842, 43.576809}, {2909.677429, 43.046653},
+ {2911.816527, 42.517279}, {2913.929176, 41.988677},
+ {2916.015414, 41.460838}, {2918.075279, 40.933752},
+ {2920.108808, 40.407407}, {2922.116038, 39.881796},
+ {2924.097005, 39.356907}, {2926.051746, 38.832731},
+ {2927.980296, 38.309259}, {2929.882689, 37.786481},
+ {2931.758961, 37.264388}, {2933.609145, 36.742970},
+ {2935.433275, 36.222219}, {2937.231383, 35.702124},
+ {2939.003503, 35.182678}, {2940.749667, 34.663871},
+ {2942.469906, 34.145695}, {2944.164252, 33.628140},
+ {2945.832736, 33.111198}, {2947.475387, 32.594860},
+ {2949.092236, 32.079119}, {2950.683314, 31.563965},
+ {2952.248647, 31.049390}, {2953.788267, 30.535386},
+ {2955.302200, 30.021946}, {2956.790475, 29.509060},
+ {2958.253120, 28.996721}, {2959.690161, 28.484922},
+ {2961.101625, 27.973654}, {2962.487539, 27.462909},
+ {2963.847929, 26.952682}, {2965.182820, 26.442962},
+ {2966.492238, 25.933745}, {2967.776207, 25.425021},
+ {2969.034752, 24.916785}, {2970.267898, 24.409029},
+ {2971.475667, 23.901745}, {2972.658084, 23.394928},
+ {2973.815171, 22.888571}, {2974.946952, 22.382666},
+ {2976.053449, 21.877207}, {2977.134684, 21.372188},
+ {2978.190679, 20.867602}, {2979.221455, 20.363444},
+ {2980.227033, 19.859706}, {2981.207436, 19.356384},
+ {2982.162682, 18.853470}, {2983.092793, 18.350960},
+ {2983.997788, 17.848848}, {2984.877687, 17.347127},
+ {2985.732510, 16.845793}, {2986.562276, 16.344841},
+ {2987.367004, 15.844264}, {2988.146712, 15.344058},
+ {2988.901419, 14.844219}, {2989.631143, 14.344740},
+ {2990.335902, 13.845618}, {2991.015713, 13.346847},
+ {2991.670595, 12.848424}, {2992.300564, 12.350344},
+ {2992.905638, 11.852603}, {2993.485833, 11.355196},
+ {2994.041166, 10.858120}, {2994.571653, 10.361371},
+ {2995.077311, 9.864946}, {2995.558156, 9.368840},
+ {2996.014203, 8.873051}, {2996.445469, 8.377575},
+ {2996.851968, 7.882409}, {2997.233717, 7.387550},
+ {2997.590731, 6.892996}, {2997.923024, 6.398744},
+ {2998.230613, 5.904790}, {2998.513511, 5.411134},
+ {2998.771734, 4.917773}, {2999.005295, 4.424704},
+ {2999.214211, 3.931926}, {2999.398495, 3.439438},
+ {2999.558162, 2.947236}, {2999.693226, 2.455321},
+ {2999.803701, 1.963691}, {2999.889602, 1.472345},
+ {2999.950943, 0.981281}, {2999.987737, 0.490500},
+ {2999.998745, 0.156943}, {2999.999950, 0.031348},
+ {2999.999998, 0.006229}, {3000.000000, 0.001206},
+ {3000.000000, 0.000201}, {3000.000000, 0.000000},
+ }},
+};
+
+} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/DeploymentController/Motor/MotorData.h b/src/boards/DeathStack/AeroBrakesController/trajectories/Trajectory.h
similarity index 58%
rename from src/boards/DeathStack/DeploymentController/Motor/MotorData.h
rename to src/boards/DeathStack/AeroBrakesController/trajectories/Trajectory.h
index 81b81c32b19e0046aba58abe04f70423a5b531b2..2009a691df9e676ff3a60c47ffb0e1e6986f5745 100644
--- a/src/boards/DeathStack/DeploymentController/Motor/MotorData.h
+++ b/src/boards/DeathStack/AeroBrakesController/trajectories/Trajectory.h
@@ -1,5 +1,6 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Alvise de' Faveri Tron
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Vincenzo Santomarco
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -16,31 +17,40 @@
* 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
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
-
#pragma once
-#include <cstdint>
+#include "Trajectories_data.h"
+#include "TrajectoryPoint.h"
namespace DeathStackBoard
{
-
-/**
- * @brief Motor direction
- */
-enum class MotorDirection : uint8_t
-{
- NORMAL,
- REVERSE
-};
-struct MotorStatus
+class Trajectory
{
- bool motor_active = false;
- MotorDirection motor_last_direction = MotorDirection::NORMAL;
+private:
+ uint32_t index;
+
+public:
+ Trajectory(uint32_t index) : index(index) {}
+ Trajectory() : index(0) {}
+
+ Trajectory& operator=(const Trajectory& other)
+ {
+ this->index = other.index;
+ return *this;
+ }
+
+ uint32_t length() { return TRAJECTORIES_DATA[index].length; }
+
+ TrajectoryPoint get(uint32_t idx)
+ {
+ point_t point = TRAJECTORIES_DATA[index].data[idx];
+ return TrajectoryPoint(point.z, point.vz);
+ }
};
-}
+} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/DeploymentController/Motor/MotorDriver.h b/src/boards/DeathStack/AeroBrakesController/trajectories/TrajectoryPoint.h
similarity index 60%
rename from src/boards/DeathStack/DeploymentController/Motor/MotorDriver.h
rename to src/boards/DeathStack/AeroBrakesController/trajectories/TrajectoryPoint.h
index c16efe2780dc00b1c3a3e8709b48e017553aa872..0355c21c2513ca8f9baa9fc1248398808f40a36e 100644
--- a/src/boards/DeathStack/DeploymentController/Motor/MotorDriver.h
+++ b/src/boards/DeathStack/AeroBrakesController/trajectories/TrajectoryPoint.h
@@ -1,6 +1,6 @@
/*
- * Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Alvise de'Faveri Tron
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Vincenzo Santomarco
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -20,51 +20,41 @@
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
-#pragma once
-#include <Common.h>
+#pragma once
-#include "configs/MotorConfig.h"
-#include "MotorData.h"
+#include <math.h>
namespace DeathStackBoard
{
-/**
- * This class gives access to the H-Bridge that controls the DC motor of the nosecone.
- */
-class MotorDriver
+class TrajectoryPoint
{
public:
- /**
- * @brief Class constructor.
- */
- MotorDriver();
+ TrajectoryPoint() : TrajectoryPoint(0, 0) {}
+ TrajectoryPoint(float z, float vz) : z(z), vz(vz) {}
- /**
- * @brief Class destructor.
- */
- ~MotorDriver();
+ float getZ() { return z; }
+ float getVz() { return vz; }
- /**
- * @brief Activates the H-Bridge to start the motor.
- *
- * @param direction direction of activation (normal or reverse)
- */
- void start(MotorDirection direction);
+ static float distance(TrajectoryPoint a, TrajectoryPoint b)
+ {
+ return powf(a.getZ() - b.getZ(), 2) + powf(a.getVz() - b.getVz(), 2);
+ }
- /**
- * @brief Stop the motor
- */
- void stop();
+ static float zDistance(TrajectoryPoint a, TrajectoryPoint b)
+ {
+ return abs(a.getZ() - b.getZ());
+ }
- MotorStatus getStatus()
+ static float vzDistance(TrajectoryPoint a, TrajectoryPoint b)
{
- return status;
+ return abs(a.getVz() - b.getVz());
}
private:
- MotorStatus status;
+ float z;
+ float vz;
};
-} /* namespace */
\ No newline at end of file
+} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/scripts/event_header_generator/Events.h.template b/src/boards/DeathStack/Algorithm.h
similarity index 54%
rename from scripts/event_header_generator/Events.h.template
rename to src/boards/DeathStack/Algorithm.h
index 3d79b58591da02b142395484b2d2865983d57a7f..0c40679515642c0c2db4aa90a08a52f8851dd30c 100644
--- a/scripts/event_header_generator/Events.h.template
+++ b/src/boards/DeathStack/Algorithm.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+/* Copyright (c) 2020 Skyward Experimental Rocketry
+ * Authors: Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -20,46 +20,51 @@
* THE SOFTWARE.
*/
-/*
- ******************************************************************************
- * THIS FILE IS AUTOGENERATED. DO NOT EDIT. *
- ******************************************************************************
- */
-
-// Generated from: {sheet_link}
-// Autogen date: {date}
-
#pragma once
-#include <cstdint>
-#include <string>
-#include <vector>
+namespace DeathStackBoard
+{
+
+class Algorithm
+{
+public:
+ /**
+ * @brief Initializes the Algorithm object, must be called as soon as the
+ * object is created.
+ * */
+ virtual bool init() = 0;
-#include "events/Event.h"
-#include "events/EventBroker.h"
-#include "Topics.h"
+ /**
+ * @brief Starts the execution of the algorithm and set the running flag to
+ * true.
+ * */
+ void begin() { running = true; }
-using std::string;
+ /**
+ * @brief Terminates the algorithm's execution and sets the running flag to
+ * false.
+ * */
+ void end() { running = false; }
-namespace DeathStackBoard
-{{
-/**
- * Definition of all events in the Homeone Board software
- * Refer to section 5.1.1 of the Software Design Document.
- */
-enum Events : uint8_t
-{{
-{enum_data}
-}};
+ /**
+ * @brief Checks wether the algorithm is in a running state or not, and
+ * eventually calls the @see{step} routine.
+ * */
+ void update()
+ {
+ if (running)
+ {
+ step();
+ }
+ }
-const std::vector<uint8_t> EVENT_LIST {{{event_list}}};
+protected:
+ /**
+ * @brief The actual algorithm step.
+ */
+ virtual void step() = 0;
-/**
- * @brief Returns the name of the provided event
- *
- * @param event
- * @return string
- */
-string getEventString(uint8_t event);
+ bool running = false;
+};
-}}
+} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/DeathStack.h b/src/boards/DeathStack/DeathStack.h
index 1d5c8b57696408acd263d9c0d0469e097951466c..4da5e855bf63f3f230147fe7f8300dd2ead54b9e 100644
--- a/src/boards/DeathStack/DeathStack.h
+++ b/src/boards/DeathStack/DeathStack.h
@@ -70,15 +70,16 @@ public:
EventSniffer* sniffer;
// FSMs
- ADAController* ada;
+ ADAController<PressureData, GPSData>* ada;
SensorManager* sensor_manager;
TMTCManager* tmtc;
FlightModeManager* fmm;
-
- Cutter* cutter;
- Servo* servo;
DeploymentController* dpl;
+ // actuators
+ //Cutter* cutter;
+ //Servo* servo;
+
private:
/**
* Initialize Everything
@@ -101,14 +102,14 @@ private:
*broker, TOPIC_LIST, bind(&DeathStack::logEvent, this, _1, _2));
}
- ada = new ADAController();
- sensor_manager = new SensorManager(ada);
+ //ada = new ADAController<PressureData, GPSData>(mock_barometer, mock_gps);
+ sensor_manager = new SensorManager();
tmtc = new TMTCManager();
fmm = new FlightModeManager();
- cutter = new Cutter(CUTTER_PWM_FREQUENCY, CUTTER_PWM_DUTY_CYCLE,
+ /*cutter = new Cutter(CUTTER_PWM_FREQUENCY, CUTTER_PWM_DUTY_CYCLE,
CUTTER_TEST_PWM_DUTY_CYCLE);
- servo = new Servo(DeploymentConfigs::SERVO_TIMER);
- dpl = new DeploymentController(*cutter, *servo);
+ servo = new Servo(DeploymentConfigs::SERVO_TIMER);*/
+ dpl = new DeploymentController(); //*cutter, *servo);
// Start threads
try
diff --git a/src/boards/DeathStack/DeploymentController/Deployment.scxml b/src/boards/DeathStack/DeploymentController/Deployment.scxml
new file mode 100644
index 0000000000000000000000000000000000000000..901844fe47cae708daa3c04bcd30ee4c60e283af
--- /dev/null
+++ b/src/boards/DeathStack/DeploymentController/Deployment.scxml
@@ -0,0 +1,50 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<scxml xmlns="http://www.w3.org/2005/07/scxml" initial="idle" version="1.0">
+ <state id="idle">
+ <onentry>initServo()</onentry>
+ <onentry>initCutters()</onentry>
+ <transition event="DPL.EV_RESET_SERVO" target="idle">
+ resetServo()
+ </transition>
+ <transition event="DPL.EV_WIGGLE_SERVO" target="idle">
+ wiggleServo()
+ </transition>
+ <transition event="DPL.EV_NC_OPEN" target="noseconeEjection"/>
+ <transition event="DPL.EV_CUT_DROGUE" target="cuttingPrimary"/>
+ <transition event="DPL.EV_TEST_CUT_PRIMARY" target="testCuttingPrimary"/>
+ <transition event="DPL.EV_TEST_CUT_BACKUP" target="testCuttingBackup"/>
+ </state>
+ <state id="noseconeEjection">
+ <onentry>ejectNosecone()</onentry>
+ <onentry>postD(DPL.EV_NC_OPEN_TIMEOUT)</onentry>
+ <onexit>disableServo()</onexit>
+ <transition event="DPL.EV_NC_DETACHED" target="idle">
+ removeD(DPL.EV_NC_OPEN_TIMEOUT)
+ </transition>
+ <transition event="DPL.EV_NC_OPEN_TIMEOUT" target="idle"/>
+ </state>
+ <state id="cuttingPrimary">
+ <onentry>enablePrimaryCutter()</onentry>
+ <onentry>postD(DPL.EV_CUTTING_TIMEOUT)</onentry>
+ <onexit>disablePrimaryCutter()</onexit>
+ <transition event="DPL.EV_CUTTING_TIMEOUT" target="cuttingBackup"/>
+ </state>
+ <state id="cuttingBackup">
+ <onentry>enableBackupCutter()</onentry>
+ <onentry>postD(DPL.EV_CUTTING_TIMEOUT)</onentry>
+ <onexit>disableBackupCutter()</onexit>
+ <transition event="DPL.EV_CUTTING_TIMEOUT" target="idle"/>
+ </state>
+ <state id="testCuttingPrimary">
+ <onentry>enablePrimaryCutter()</onentry>
+ <onentry>postD(DPL.EV_CUTTING_TIMEOUT)</onentry>
+ <onexit>disablePrimaryCutter()</onexit>
+ <transition event="DPL.EV_CUTTING_TIMEOUT" target="idle"/>
+ </state>
+ <state id="testCuttingBackup">
+ <onentry>enableBackupCutter()</onentry>
+ <onentry>postD(DPL.EV_CUTTING_TIMEOUT)</onentry>
+ <onexit>disableBackupCutter()</onexit>
+ <transition event="DPL.EV_CUTTING_TIMEOUT" target="idle"/>
+ </state>
+</scxml>
\ No newline at end of file
diff --git a/src/boards/DeathStack/DeploymentController/DeploymentController.cpp b/src/boards/DeathStack/DeploymentController/DeploymentController.cpp
index 64b7bc733c97e52b5f20fdb1cd4457458844ebe8..b459bfe5f52cc9d9890347023dbb6213edae6ee1 100644
--- a/src/boards/DeathStack/DeploymentController/DeploymentController.cpp
+++ b/src/boards/DeathStack/DeploymentController/DeploymentController.cpp
@@ -1,6 +1,6 @@
/*
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Luca Erbetta, Alvise de' Faveri Tron
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -21,30 +21,24 @@
* THE SOFTWARE.
*/
-#include <stdexcept>
-
-#include "configs/DeploymentConfig.h"
-#include "events/Events.h"
#include "DeploymentController.h"
-#include "Motor/MotorDriver.h"
+#include "TimestampTimer.h"
+#include "configs/DeploymentConfig.h"
#include "events/EventBroker.h"
+#include "events/Events.h"
namespace DeathStackBoard
{
-using namespace DeploymentConfigs;
-
-DeploymentController::DeploymentController(Cutter& cutter,
- Servo& ejection_servo)
- : HSM(&DeploymentController::state_initialization, 4096, 2),
- cutters(cutter), ejection_servo(ejection_servo)
+DeploymentController::DeploymentController(HBridge* primaryCutter,
+ HBridge* backupCutter,
+ ServoInterface* ejection_servo)
+ : FSM(&DeploymentController::state_initialization),
+ primaryCutter{primaryCutter}, backupCutter{backupCutter},
+ ejection_servo{ejection_servo}
{
- memset(&status, 0, sizeof(DeploymentStatus));
-
- sEventBroker->subscribe(this, TOPIC_DEPLOYMENT);
- sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS);
- sEventBroker->subscribe(this, TOPIC_TC);
+ sEventBroker->subscribe(this, TOPIC_DPL);
}
DeploymentController::~DeploymentController()
@@ -52,375 +46,290 @@ DeploymentController::~DeploymentController()
sEventBroker->unsubscribe(this);
}
-State DeploymentController::state_initialization(const Event& ev)
+void DeploymentController::logStatus(DeploymentControllerState current_state)
{
- initServo();
+ status.timestamp = TimestampTimer::getTimestamp();
+ status.state = current_state;
+ status.servo_position = ejection_servo->getCurrentPosition();
+ status.primary_cutter_state = primaryCutter->getStatus();
+ status.backup_cutter_state = backupCutter->getStatus();
+
+ // logger.log(status);
- UNUSED(ev);
- return transition(&DeploymentController::state_idle);
+ // StackLogger::getInstance()->updateStack(THID_DPL_FSM);
}
-State DeploymentController::state_idle(const Event& ev)
+void DeploymentController::state_initialization(const Event& ev)
{
- State retState = HANDLED;
switch (ev.sig)
{
case EV_ENTRY:
{
- logStatus(DeploymentCTRLState::DPL_IDLE);
- // Process deferred events
- try
- {
- while (!deferred_events.isEmpty())
- {
- postEvent(deferred_events.pop());
- }
- }
- catch (...)
- {
- TRACE("Tried to pop empty circularbuffer!\n");
- }
+ ejection_servo->enable();
+ ejection_servo->reset();
- TRACE("[DPL_CTRL] state_idle ENTRY\n");
+ transition(&DeploymentController::state_idle);
break;
}
- case EV_INIT:
+ case EV_EXIT:
{
break;
}
- case EV_EXIT:
- {
- TRACE("[DPL_CTRL] state_idle EXIT\n");
+ default:
+ {
break;
}
- case EV_NC_OPEN:
+ }
+}
+
+void DeploymentController::state_idle(const Event& ev)
+{
+ switch (ev.sig)
+ {
+ case EV_ENTRY:
{
- retState =
- transition(&DeploymentController::state_ejectingNosecone);
+ logStatus(DeploymentControllerState::IDLE);
+
+ TRACE("[DPL] entering state idle\n");
break;
}
- case EV_CUT_DROGUE:
+ case EV_EXIT:
{
- cut_backup = true;
- retState = transition(&DeploymentController::state_cuttingPrimary);
+ TRACE("[DPL] exiting state idle\n");
break;
}
- case EV_CUT_PRIMARY:
+ case EV_RESET_SERVO:
{
- cut_backup = false;
- retState = transition(&DeploymentController::state_cuttingPrimary);
+ TRACE("[DPL] reset servo \n");
+ ejection_servo->reset();
break;
}
- case EV_CUT_BACKUP:
+ case EV_WIGGLE_SERVO:
{
- retState = transition(&DeploymentController::state_cuttingBackup);
+ TRACE("[DPL] wiggle servo \n");
+ ejection_servo->selfTest();
break;
}
- case EV_TEST_CUTTER_PRIMARY:
+ case EV_NC_OPEN:
{
- retState = transition(&DeploymentController::state_testingPrimary);
+ TRACE("[DPL] nosecone open event \n");
+ transition(&DeploymentController::state_noseconeEjection);
break;
}
- case EV_TEST_CUTTER_BACKUP:
+ case EV_CUT_DROGUE:
{
- retState = transition(&DeploymentController::state_testingBackup);
+ TRACE("[DPL] cut drogue event \n");
+ transition(&DeploymentController::state_cuttingPrimary);
break;
}
- case EV_RESET_SERVO:
+ case EV_TEST_CUT_PRIMARY:
{
- resetServo();
+ transition(&DeploymentController::state_testCuttingPrimary);
break;
}
- case EV_WIGGLE_SERVO:
+ case EV_TEST_CUT_BACKUP:
{
- wiggleServo();
+ transition(&DeploymentController::state_testCuttingBackup);
break;
}
default:
{
- retState = tran_super(&DeploymentController::Hsm_top);
break;
}
}
- return retState;
}
-State DeploymentController::state_ejectingNosecone(const Event& ev)
+void DeploymentController::state_noseconeEjection(const Event& ev)
{
- State retState = HANDLED;
switch (ev.sig)
{
case EV_ENTRY:
{
- ejectNosecone();
- ev_open_timeout_id = sEventBroker->postDelayed<NC_OPEN_TIMEOUT>(
- Event{EV_TIMEOUT_NC_OPEN}, TOPIC_DEPLOYMENT);
+ TRACE("[DPL] entering state nosecone_ejection\n");
- logStatus(DeploymentCTRLState::EJECTING_NC);
- TRACE("[DPL_CTRL] state_ejectingNosecone ENTRY\n");
- break;
- }
- case EV_INIT:
- {
- TRACE("[DPL_CTRL] state_ejectingNosecone INIT\n");
+ ejection_servo->set(SERVO_EJECT_POS);
+
+ ev_nc_open_timeout_id = sEventBroker->postDelayed<NC_OPEN_TIMEOUT>(
+ Event{EV_NC_OPEN_TIMEOUT}, TOPIC_DPL);
+
+ logStatus(DeploymentControllerState::NOSECONE_EJECTION);
break;
}
case EV_EXIT:
{
- // disableServo();
- sEventBroker->removeDelayed(ev_open_timeout_id);
-
- TRACE("[DPL_CTRL] state_openingNosecone EXIT\n");
-
+ TRACE("[DPL] exiting state nosecone_ejection\n");
break;
}
- case EV_CUT_DROGUE:
+ case EV_NC_DETACHED:
{
- deferred_events.put(ev);
+ TRACE("[DPL] nosecone detached event \n");
+ sEventBroker->removeDelayed(ev_nc_open_timeout_id);
+ transition(&DeploymentController::state_idle);
break;
}
- case EV_NC_DETACHED:
- case EV_TIMEOUT_NC_OPEN:
+ case EV_NC_OPEN_TIMEOUT:
{
- retState = transition(&DeploymentController::state_idle);
+ TRACE("[DPL] nosecone opening timeout \n");
+ transition(&DeploymentController::state_idle);
break;
}
default:
{
- retState = tran_super(&DeploymentController::Hsm_top);
break;
}
}
- return retState;
}
-State DeploymentController::state_cuttingPrimary(const Event& ev)
+void DeploymentController::state_cuttingPrimary(const Event& ev)
{
- State retState = HANDLED;
switch (ev.sig)
{
case EV_ENTRY:
{
- // Cutting drogue
- cutters.enablePrimaryCutter();
+ TRACE("[DPL] entering state cutting_primary\n");
- logStatus(DeploymentCTRLState::CUTTING_PRIMARY);
+ primaryCutter->enable();
- ev_cut_timeout_id = sEventBroker->postDelayed<CUT_DURATION>(
- {EV_TIMEOUT_CUTTING}, TOPIC_DEPLOYMENT);
+ ev_nc_cutting_timeout_id = sEventBroker->postDelayed<CUT_DURATION>(
+ Event{EV_CUTTING_TIMEOUT}, TOPIC_DPL);
+
+ logStatus(DeploymentControllerState::CUTTING_PRIMARY);
- TRACE("[DPL_CTRL] state_cuttingPrimary ENTRY\n");
- break;
- }
- case EV_INIT:
- {
break;
}
case EV_EXIT:
{
- cutters.disablePrimaryCutter();
+ TRACE("[DPL] exiting state cutting_primary\n");
- sEventBroker->removeDelayed(ev_cut_timeout_id);
- TRACE("[DPL_CTRL] state_cuttingPrimary EXIT\n");
+ primaryCutter->disable();
break;
}
- case EV_TIMEOUT_CUTTING:
+ case EV_CUTTING_TIMEOUT:
{
- if (cut_backup)
- {
- retState =
- transition(&DeploymentController::state_cuttingBackup);
- }
- else
- {
- retState = transition(&DeploymentController::state_idle);
- }
+ TRACE("[DPL] primary cutter timeout \n");
+ transition(&DeploymentController::state_cuttingBackup);
break;
}
default:
{
- retState = tran_super(&DeploymentController::Hsm_top);
break;
}
}
- return retState;
}
-State DeploymentController::state_cuttingBackup(const Event& ev)
+void DeploymentController::state_cuttingBackup(const Event& ev)
{
- State retState = HANDLED;
switch (ev.sig)
{
case EV_ENTRY:
{
- // Cutting rogallina
- cutters.enableBackupCutter();
+ TRACE("[DPL] entering state cutting_backup\n");
- logStatus(DeploymentCTRLState::CUTTING_BACKUP);
+ backupCutter->enable();
- ev_cut_timeout_id = sEventBroker->postDelayed<CUT_DURATION>(
- {EV_TIMEOUT_CUTTING}, TOPIC_DEPLOYMENT);
+ ev_nc_cutting_timeout_id = sEventBroker->postDelayed<CUT_DURATION>(
+ Event{EV_CUTTING_TIMEOUT}, TOPIC_DPL);
+
+ logStatus(DeploymentControllerState::CUTTING_BACKUP);
- TRACE("[DPL_CTRL] state_cuttingBackup ENTRY\n");
- break;
- }
- case EV_INIT:
- {
break;
}
case EV_EXIT:
{
- cutters.disableBackupCutter();
+ TRACE("[DPL] exiting state cutting_backup\n");
- sEventBroker->removeDelayed(ev_cut_timeout_id);
-
- TRACE("[DPL_CTRL] state_cuttingBackup EXIT\n");
+ backupCutter->disable();
break;
}
- case EV_TIMEOUT_CUTTING:
+ case EV_CUTTING_TIMEOUT:
{
- retState = transition(&DeploymentController::state_idle);
+ TRACE("[DPL] backup cutter timeout \n");
+ transition(&DeploymentController::state_idle);
break;
}
default:
{
- retState = tran_super(&DeploymentController::Hsm_top);
break;
}
}
- return retState;
}
-State DeploymentController::state_testingPrimary(const Event& ev)
+void DeploymentController::state_testCuttingPrimary(const Event& ev)
{
- State retState = HANDLED;
switch (ev.sig)
{
case EV_ENTRY:
{
- cutters.enableTestPrimaryCutter();
+ TRACE("[DPL] entering state test_cutting_primary\n");
- logStatus(DeploymentCTRLState::TESTING_PRIMARY);
+ primaryCutter->enableTest(CUTTER_TEST_PWM_DUTY_CYCLE);
- ev_cut_timeout_id = sEventBroker->postDelayed<CUT_TEST_DURATION>(
- {EV_TIMEOUT_CUTTING}, TOPIC_DEPLOYMENT);
+ ev_nc_cutting_timeout_id =
+ sEventBroker->postDelayed<CUT_TEST_DURATION>(
+ Event{EV_CUTTING_TIMEOUT}, TOPIC_DPL);
- TRACE("[DPL_CTRL] state_testingPrimary ENTRY\n");
- break;
- }
- case EV_INIT:
- {
+ logStatus(DeploymentControllerState::TEST_CUTTING_PRIMARY);
break;
}
case EV_EXIT:
{
- cutters.disablePrimaryCutter();
-
- sEventBroker->removeDelayed(ev_cut_timeout_id);
-
- TRACE("[DPL_CTRL] state_testingPrimary EXIT\n");
+ TRACE("[DPL] exiting state test_cutting_primary\n");
+ primaryCutter->disable();
+
break;
}
- case EV_TIMEOUT_CUTTING:
+ case EV_CUTTING_TIMEOUT:
{
- retState = transition(&DeploymentController::state_idle);
+ TRACE("[DPL] test primary cutter timeout \n");
+ transition(&DeploymentController::state_idle);
break;
}
default:
{
- retState = tran_super(&DeploymentController::Hsm_top);
break;
}
}
- return retState;
}
-State DeploymentController::state_testingBackup(const Event& ev)
+void DeploymentController::state_testCuttingBackup(const Event& ev)
{
- State retState = HANDLED;
switch (ev.sig)
{
case EV_ENTRY:
{
- // Cutting rogallina
- cutters.enableTestBackupCutter();
+ backupCutter->enableTest(CUTTER_TEST_PWM_DUTY_CYCLE);
- logStatus(DeploymentCTRLState::TESTING_BACKUP);
+ ev_nc_cutting_timeout_id =
+ sEventBroker->postDelayed<CUT_TEST_DURATION>(
+ Event{EV_CUTTING_TIMEOUT}, TOPIC_DPL);
- ev_cut_timeout_id = sEventBroker->postDelayed<CUT_TEST_DURATION>(
- {EV_TIMEOUT_CUTTING}, TOPIC_DEPLOYMENT);
+ logStatus(DeploymentControllerState::TEST_CUTTING_BACKUP);
- TRACE("[DPL_CTRL] state_testingBackup ENTRY\n");
- break;
- }
- case EV_INIT:
- {
+ TRACE("[DPL] entering state test_cutting_backup\n");
break;
}
case EV_EXIT:
{
- cutters.disableBackupCutter();
-
- sEventBroker->removeDelayed(ev_cut_timeout_id);
-
- TRACE("[DPL_CTRL] state_testingBackup EXIT\n");
+ backupCutter->disable();
+ TRACE("[DPL] exiting state test_cutting_backup\n");
break;
}
- case EV_TIMEOUT_CUTTING:
+ case EV_CUTTING_TIMEOUT:
{
- retState = transition(&DeploymentController::state_idle);
+ TRACE("[DPL] test backup cutter timeout \n");
+ transition(&DeploymentController::state_idle);
break;
}
default:
{
- retState = tran_super(&DeploymentController::Hsm_top);
break;
}
}
- return retState;
-}
-
-void DeploymentController::initServo()
-{
- ejection_servo.enable(SERVO_CHANNEL);
-
- ejection_servo.setPosition(SERVO_CHANNEL, SERVO_RESET_POS);
-
- ejection_servo.start();
-}
-
-void DeploymentController::resetServo()
-{
- ejection_servo.setPosition(SERVO_CHANNEL, SERVO_RESET_POS);
-}
-
-void DeploymentController::ejectNosecone()
-{
- ejection_servo.setPosition(SERVO_CHANNEL, SERVO_EJECT_POS);
-}
-
-void DeploymentController::disableServo()
-{
- // Do not reset the position to 0
-
- ejection_servo.stop();
- 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);
- Thread::sleep(500);
- }
}
-} // namespace DeathStackBoard
+} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/DeploymentController/DeploymentController.h b/src/boards/DeathStack/DeploymentController/DeploymentController.h
index 1b8b55cb82f555289dafc56dc3dd0dd60b172517..3399eefa1b8d6a29a43750627b386401f322535a 100644
--- a/src/boards/DeathStack/DeploymentController/DeploymentController.h
+++ b/src/boards/DeathStack/DeploymentController/DeploymentController.h
@@ -1,6 +1,6 @@
/*
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -24,100 +24,57 @@
#pragma once
#include <drivers/servo/servo.h>
-#include "LoggerService/LoggerService.h"
-#include "System/StackLogger.h"
-#include "configs/DeploymentConfig.h"
+
#include "DeploymentData.h"
-#include "Motor/MotorDriver.h"
-#include "ThermalCutter/Cutter.h"
-#include "events/HSM.h"
-#include "utils/collections/CircularBuffer.h"
+#include "DeploymentServo.h"
+#include "configs/CutterConfig.h"
+#include "drivers/hbridge/HBridge.h"
+#include "events/FSM.h"
+#include "events/Events.h"
-class PinObserver;
+using namespace DeathStackBoard::DeploymentConfigs;
+using namespace DeathStackBoard::CutterConfig;
namespace DeathStackBoard
{
/**
- * @brief Deployment Controller State Machine
+ * @brief Deployment state machine
*/
-class DeploymentController : public HSM<DeploymentController>
+class DeploymentController : public FSM<DeploymentController>
{
public:
- DeploymentController(Cutter &cutter, Servo &ejection_servo);
+ DeploymentController(
+ HBridge* primaryCutter = new HBridge(PrimaryCutterEna::getPin(),
+ CUTTER_TIM, CUTTER_CHANNEL_PRIMARY,
+ PRIMARY_CUTTER_PWM_FREQUENCY,
+ PRIMARY_CUTTER_PWM_DUTY_CYCLE),
+ HBridge* backupCutter = new HBridge(BackupCutterEna::getPin(),
+ CUTTER_TIM, CUTTER_CHANNEL_PRIMARY,
+ BACKUP_CUTTER_PWM_FREQUENCY,
+ BACKUP_CUTTER_PWM_DUTY_CYCLE),
+ ServoInterface* ejection_servo = new DeploymentServo());
~DeploymentController();
- State state_initialization(const Event &ev);
-
- State state_idle(const Event &ev);
-
- State state_cuttingPrimary(const Event &ev);
- State state_cuttingBackup(const Event &ev);
-
- State state_testingPrimary(const Event &ev);
- State state_testingBackup(const Event &ev);
-
- State state_ejectingNosecone(const Event &ev);
+ void state_initialization(const Event& ev);
+ void state_idle(const Event& ev);
+ void state_noseconeEjection(const Event& ev);
+ void state_cuttingPrimary(const Event& ev);
+ void state_cuttingBackup(const Event& ev);
+ void state_testCuttingPrimary(const Event& ev);
+ void state_testCuttingBackup(const Event& ev);
private:
- /**
- * @brief Logs the DeploymentStatus struct updating the timestamp and the
- * current state
- *
- * @param current_state
- */
- void logStatus(DeploymentCTRLState current_state)
- {
- status.state = current_state;
- logStatus();
- }
- /**
- * @brief Logs the DeploymentStatus struct updating the timestamp
- */
- void logStatus()
- {
- status.timestamp = miosix::getTick();
- status.cutter_status = cutters.getStatus();
- status.servo_position =
- ejection_servo.getPosition(DeploymentConfigs::SERVO_CHANNEL);
-
- logger.log(status);
- StackLogger::getInstance()->updateStack(THID_DPL_FSM);
- }
-
- void initServo();
- void resetServo();
- 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
- *
- * @param ev The event to be defered.
- */
- void deferEvent(const Event &ev);
-
- Cutter &cutters;
- Servo &ejection_servo;
-
DeploymentStatus status;
- LoggerService &logger = *(LoggerService::getInstance());
-
- CircularBuffer<Event, DEFERRED_EVENTS_QUEUE_SIZE> deferred_events;
+ HBridge* primaryCutter;
+ HBridge* backupCutter;
+ ServoInterface* ejection_servo;
- bool cut_backup = true;
+ uint16_t ev_nc_open_timeout_id = 0;
+ uint16_t ev_nc_cutting_timeout_id = 0;
- uint16_t ev_open_timeout_id = 0;
- uint16_t ev_reset_timeout_id = 0;
- uint16_t ev_cut_timeout_id = 0;
+ void logStatus(DeploymentControllerState current_state);
};
-} // namespace DeathStackBoard
\ No newline at end of file
+} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/DeploymentController/DeploymentData.h b/src/boards/DeathStack/DeploymentController/DeploymentData.h
index 9b0d8a46461caef4c484a05073a5afe73af73b6d..80d4410ebcad5cd48f30a1856fa061eba5e19d96 100644
--- a/src/boards/DeathStack/DeploymentController/DeploymentData.h
+++ b/src/boards/DeathStack/DeploymentController/DeploymentData.h
@@ -1,6 +1,6 @@
/*
- * Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -23,39 +23,52 @@
#pragma once
-#include "Motor/MotorData.h"
-#include "ThermalCutter/CutterData.h"
+#include <stdint.h>
+
+#include <iostream>
+#include <string>
+
+#include "drivers/hbridge/HBridgeData.h"
namespace DeathStackBoard
{
-enum DeploymentCTRLState : uint8_t
+/**
+ * Enum defining the possibile FSM states.
+ */
+enum DeploymentControllerState : uint8_t
{
- DPL_IDLE = 0,
+ IDLE = 0,
+ NOSECONE_EJECTION,
CUTTING_PRIMARY,
CUTTING_BACKUP,
- EJECTING_NC,
- TESTING_PRIMARY,
- TESTING_BACKUP
+ TEST_CUTTING_PRIMARY,
+ TEST_CUTTING_BACKUP,
};
+/**
+ * Structure defining the overall controller status.
+ */
struct DeploymentStatus
{
- long long timestamp;
- DeploymentCTRLState state;
- CutterStatus cutter_status;
+ uint64_t timestamp;
+ DeploymentControllerState state;
+ HBridgeStatus primary_cutter_state;
+ HBridgeStatus backup_cutter_state;
float servo_position;
static std::string header()
{
- return "timestamp,state,cutter_state,servo_position\n";
+ return "timestamp,state,primary_cutter_state,backup_cutter_state,servo_"
+ "position\n";
}
void print(std::ostream& os) const
{
- os << timestamp << "," << (int)state << "," << (int)cutter_status.state
- << "," << servo_position << "\n";
+ os << timestamp << "," << (int)state << ","
+ << (int)primary_cutter_state.state << ","
+ << (int)backup_cutter_state.state << "," << servo_position << "\n";
}
};
-} // namespace DeathStackBoard
\ No newline at end of file
+} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/DeploymentController/DeploymentServo.h b/src/boards/DeathStack/DeploymentController/DeploymentServo.h
new file mode 100644
index 0000000000000000000000000000000000000000..c6065c47026cf41454ee07887d25fdfb716ed44e
--- /dev/null
+++ b/src/boards/DeathStack/DeploymentController/DeploymentServo.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio
+ *
+ * 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 "../ServoInterface.h"
+#include "../configs/DeploymentConfig.h"
+#include "Constants.h"
+#include "drivers/servo/servo.h"
+#include "miosix.h"
+
+using namespace DeathStackBoard::DeploymentConfigs;
+
+namespace DeathStackBoard
+{
+
+class DeploymentServo : public ServoInterface
+{
+public:
+ Servo servo{SERVO_TIMER};
+
+ DeploymentServo()
+ : ServoInterface(SERVO_MIN_POS, SERVO_MAX_POS, SERVO_RESET_POS)
+ {
+ }
+ DeploymentServo(float minPosition, float maxPosition, float resetPosition)
+ : ServoInterface(minPosition, maxPosition, resetPosition)
+ {
+ }
+
+ void enable() override
+ {
+ servo.enable(SERVO_PWM_CH);
+ servo.start();
+ }
+
+ void disable() override
+ {
+ servo.stop();
+ servo.disable(SERVO_PWM_CH);
+ }
+
+ /**
+ * @brief Perform wiggle around the reset position.
+ */
+ void selfTest() override
+ {
+ for (int i = 0; i < 3; i++)
+ {
+ set(SERVO_RESET_POS - SERVO_WIGGLE_AMPLITUDE);
+ miosix::Thread::sleep(500);
+ reset();
+ miosix::Thread::sleep(500);
+ }
+ }
+
+protected:
+ void setPosition(float angle) override
+ {
+ servo.setPosition(SERVO_PWM_CH, angle / 180.0f);
+ }
+
+private:
+ float anglePrec = SERVO_RESET_POS;
+};
+
+} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/DeploymentController/Motor/MotorDriver.cpp b/src/boards/DeathStack/DeploymentController/Motor/MotorDriver.cpp
deleted file mode 100644
index 00182f10cbcdd21c9c70db560dbb70e0895b243d..0000000000000000000000000000000000000000
--- a/src/boards/DeathStack/DeploymentController/Motor/MotorDriver.cpp
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
- * Copyright (c) 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.
- */
-#include <Common.h>
-#include <interfaces-impl/hwmapping.h>
-
-#include "events/Events.h"
-#include "MotorDriver.h"
-
-using namespace miosix;
-
-namespace DeathStackBoard
-{
-
-MotorDriver::MotorDriver() { status.motor_active = 0; }
-
-MotorDriver::~MotorDriver() { stop(); }
-
-void MotorDriver::start(MotorDirection direction)
-{
- // Ensure the pins are configured in output mode. (The rogallo controller
- // may set these pins to "alternate mode" to use PWM to drive the servos)
- nosecone::motP1::mode(Mode::OUTPUT);
- nosecone::motP2::mode(Mode::OUTPUT);
- nosecone::motP1::low();
- nosecone::motP2::low();
-
- if (direction == MotorDirection::NORMAL)
- {
- nosecone::motP1::high();
- nosecone::motP2::low();
- }
- else
- {
- nosecone::motP1::low();
- nosecone::motP2::high();
- }
-
- /* Activate PWM on both half bridges */
- nosecone::motEn::high();
-
- /* Update status */
- status.motor_active = true;
- status.motor_last_direction = direction;
-}
-
-void MotorDriver::stop()
-{
- nosecone::motP1::low();
- nosecone::motP2::low();
-
- Thread::sleep(MOTOR_DISABLE_DELAY_MS); // Wait a short delay
-
- nosecone::motEn::low();
-
- status.motor_active = false;
-}
-
-} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/DeploymentController/README.md b/src/boards/DeathStack/DeploymentController/README.md
deleted file mode 100644
index 8ed3ce0def64db9b85ce94d569f4ac58573e5219..0000000000000000000000000000000000000000
--- a/src/boards/DeathStack/DeploymentController/README.md
+++ /dev/null
@@ -1,25 +0,0 @@
- ```plantuml
-skinparam titleFontSize 34
-
-title Deployment State Machine
-
-state Ready {
- [*] --> CutterIdle
-
- state CutterIdle
- state CuttingDrogue
- state CuttingMain
- state C <<choice>>
-}
-
-[*] --> Ready
-CutterIdle --> CuttingMain : EV_TC_CUT_MAIN
-CutterIdle -r-> CuttingDrogue : EV_CUT_DROGUE
-
-CuttingDrogue --> C : EV_TIMEOUT_CUTTING
-C --> CuttingMain : [cut_main = true]
-C --> CutterIdle : [cut_main = false]
-
-CuttingMain --> CutterIdle : EV_TIMEOUT_CUTTING
-
- ```
\ No newline at end of file
diff --git a/src/boards/DeathStack/DeploymentController/ThermalCutter/Cutter.h b/src/boards/DeathStack/DeploymentController/ThermalCutter/Cutter.h
deleted file mode 100644
index 68861d9b9fa4bff4583ce61872569c36003494d5..0000000000000000000000000000000000000000
--- a/src/boards/DeathStack/DeploymentController/ThermalCutter/Cutter.h
+++ /dev/null
@@ -1,184 +0,0 @@
-/*
- * Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
- *
- * 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 <miosix.h>
-
-#include "CutterData.h"
-#include "configs/CutterConfig.h"
-#include "drivers/pwm/pwm.h"
-
-using miosix::GpioPin;
-using miosix::Thread;
-
-namespace DeathStackBoard
-{
-
-/**
- * @brief Interface class to operate the thermal cutters on the Hermev V1
- * rocket.
- *
- * Provides the ability to enable a cutter using the "nominal" duty cycle or
- * using a "test" duty cycle (lower duty cycle that does not cut the parachute
- * but is used to check if current is flowing)
- */
-class Cutter
-{
-public:
- /**
- * @brief Create a new Cutter
- *
- * @param frequency Frequency of the PWM driving the cutters
- * @param duty_cycle Duty cycle of the PWM driving the cutters
- * @param test_duty_cycle Duty cycle to be used when testing the
- * cutters for continuity
- */
- Cutter(unsigned int frequency, float duty_cycle, float test_duty_cycle)
- : pwm(CUTTER_TIM, frequency),
- pin_enable_primary(PrimaryCutterEna::getPin()),
- pin_enable_backup(BackupCutterEna::getPin()),
- cut_duty_cycle(duty_cycle), test_duty_cycle(test_duty_cycle)
- {
- pin_enable_primary.low();
- pin_enable_backup.low();
-
- // Start PWM with 0 duty cycle to keep IN pins low
- pwm.enableChannel(CUTTER_CHANNEL_PRIMARY, 0.0f);
- pwm.enableChannel(CUTTER_CHANNEL_BACKUP, 0.0f);
-
- pwm.start();
- }
-
- ~Cutter()
- {
- disablePrimaryCutter();
- disableBackupCutter();
-
- pwm.stop();
- }
-
- /**
- * @brief Activates the primary cutter.
- */
- void enablePrimaryCutter()
- {
- enableCutter(CUTTER_CHANNEL_PRIMARY, pin_enable_primary,
- cut_duty_cycle);
- status.state = CutterState::CUTTING_PRIMARY;
- }
-
- /**
- * @brief Deactivates the primary cutter
- */
- void disablePrimaryCutter()
- {
- if (status.state == CutterState::CUTTING_PRIMARY)
- {
- disableCutter(CUTTER_CHANNEL_PRIMARY, pin_enable_primary);
- status.state = CutterState::IDLE;
- }
- }
-
- /**
- * @brief Activates the backup cutter.
- */
- void enableBackupCutter()
- {
- enableCutter(CUTTER_CHANNEL_BACKUP, pin_enable_backup, cut_duty_cycle);
- status.state = CutterState::CUTTING_BACKUP;
- }
-
- /**
- * @brief Deactivates the pbackup cutter
- */
- void disableBackupCutter()
- {
- if (status.state == CutterState::CUTTING_BACKUP)
- {
- disableCutter(CUTTER_CHANNEL_BACKUP, pin_enable_backup);
- status.state = CutterState::IDLE;
- }
- }
-
- /**
- * @brief Enables the primary cutter using the "test" duty cycle
- *
- * call disablePrimaryCutter() to disable
- */
- void enableTestPrimaryCutter()
- {
- enableCutter(CUTTER_CHANNEL_PRIMARY, pin_enable_primary, test_duty_cycle);
- status.state = CutterState::TESTING_PRIMARY;
- }
-
- /**
- * @brief Enables the backup cutter using the "test" duty cycle
- *
- * call disableBackupCutter() to disable
- */
- void enableTestBackupCutter()
- {
- enableCutter(CUTTER_CHANNEL_BACKUP, pin_enable_backup, test_duty_cycle);
- status.state = CutterState::TESTING_BACKUP;
- }
-
- CutterStatus getStatus() { return status; }
-
-private:
- void enableCutter(PWMChannel channel, miosix::GpioPin& ena_pin,
- float duty_cycle)
- {
- // Only enable if the cutter is currently idle
- if (status.state == CutterState::IDLE)
- {
- // Enable PWM Generation
- pwm.setDutyCycle(channel, duty_cycle);
-
- // enable
- ena_pin.high();
- }
- }
-
- void disableCutter(PWMChannel channel, miosix::GpioPin& ena_pin)
- {
- pwm.setDutyCycle(channel, 0.0f); // Set duty cycle to 0 to leave the IN
- // pin of the Half-H bridge low
-
- Thread::sleep(CUTTER_DISABLE_DELAY_MS); // Wait a short delay
-
- ena_pin.low(); // Disable hbridge
- }
-
- Cutter(const Cutter& c) = delete;
-
- PWM pwm;
- GpioPin pin_enable_primary;
- GpioPin pin_enable_backup;
-
- float cut_duty_cycle;
- float test_duty_cycle;
- CutterStatus status;
-};
-
-} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/DeploymentController/ThermalCutter/PidCutter.h b/src/boards/DeathStack/DeploymentController/ThermalCutter/PidCutter.h
deleted file mode 100644
index 7ec7e1e4006d8761241aeec5659353c29656f818..0000000000000000000000000000000000000000
--- a/src/boards/DeathStack/DeploymentController/ThermalCutter/PidCutter.h
+++ /dev/null
@@ -1,109 +0,0 @@
-/**
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
- *
- * 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 <miosix.h>
-
-#include "ActiveObject.h"
-#include "CutterData.h"
-#include "configs/CutterConfig.h"
-#include "drivers/pwm/pwm.h"
-
-using miosix::GpioPin;
-using miosix::Thread;
-
-namespace DeathStackBoard
-{
-class PidCutter : public ActiveObject
-{
-public:
- PidCutter()
- : pwm(CUTTER_TIM, CUTTER_PWM_FREQUENCY),
- pin_enable_drogue(DrogueCutterEna::getPin()),
- pin_enable_main_chute(MainChuteCutterEna::getPin())
- {
- pin_enable_drogue.low();
- pin_enable_main_chute.low();
-
- // Start PWM with 0 duty cycle to keep IN pins low
- pwm.enableChannel(CUTTER_CHANNEL_DROGUE, 0.0f);
- pwm.enableChannel(CUTTER_CHANNEL_MAIN_CHUTE, 0.0f);
-
- pwm.start();
- }
-
- ~PidCutter()
- {
- stopCutDrogue();
- stopCutMainChute();
- pwm.stop();
- }
-
- void startCutDrogue() { cut_drogue = true; }
- void stopCutDrogue() { cut_drogue = false; }
- void startCutMainChute() { cut_main = true; }
- void stopCutMainChute() { cut_main = false; }
-
- void updateCut1Current(float current) { cut_1_curr = current; }
- void updateCut2Current(float current) { cut_2_curr = current; }
-
-protected:
- void run() override
- {
- for (;;)
- {
- if (cut_drogue)
- {
- float err = cut_1_curr - CUT_1_TARGET_CURR;
- pwm.setDutyCycle(CUTTER_CHANNEL_DROGUE, err * PID_K);
- }
- else if (cut_main)
- {
- float err = cut_2_curr - CUT_2_TARGET_CURR;
- pwm.setDutyCycle(CUTTER_CHANNEL_MAIN_CHUTE, err * PID_K);
- }
- else
- {
- pwm.setDutyCycle(CUTTER_CHANNEL_DROGUE, 0.0f);
- pwm.setDutyCycle(CUTTER_CHANNEL_MAIN_CHUTE, 0.0f);
- }
-
- Thread::sleep(50);
- }
- }
-
-private:
- float cut_1_curr = 0;
- float cut_2_curr = 0;
- bool cut_drogue = false;
- bool cut_main = false;
-
- PWM pwm;
-
- GpioPin pin_enable_drogue;
- GpioPin pin_enable_main_chute;
-
- CutterStatus status;
-};
-} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/DeploymentController/readme.md b/src/boards/DeathStack/DeploymentController/readme.md
new file mode 100644
index 0000000000000000000000000000000000000000..22794a437f9ec1bc87830603bfb04423dd344d8a
--- /dev/null
+++ b/src/boards/DeathStack/DeploymentController/readme.md
@@ -0,0 +1,14 @@
+```mermaid
+stateDiagram-v2
+ [*] --> idle
+ idle --> nosecone_ejection : EV_NC_OPEN
+ idle --> cutting_primary : EV_CUT_DROGUE
+ idle --> test_cutting_primary : EV_TEST_CUT_PRIMARY
+ idle --> test_cutting_backup : EV_TEST_CUT_BACKUP
+ nosecone_ejection --> idle : EV_NC_DETACHED
+ nosecone_ejection --> idle : EV_NC_OPEN_TIMEOUT
+ cutting_primary --> cutting_backup : EV_CUTTING_TIMEOUT
+ cutting_backup --> idle : EV_CUTTING_TIMEOUT
+ test_cutting_primary --> idle : EV_CUTTING_TIMEOUT
+ test_cutting_backup --> idle : EV_CUTTING_TIMEOUT
+```
diff --git a/src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp b/src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp
index 68a9a870cc79103fc3cd4729340bcd45d301e53d..0ed8240d7dd95a2726015bf0051fdef94b561c71 100644
--- a/src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/DeathStack/FlightModeManager/FlightModeManager.cpp
@@ -380,42 +380,42 @@ State FlightModeManager::state_testing(const Event& ev)
}
case EV_TC_NC_OPEN:
{
- sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DPL);
break;
}
case EV_TC_CUT_DROGUE:
{
- sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DPL);
break;
}
case EV_TC_CUT_PRIMARY:
{
- sEventBroker->post(Event{EV_CUT_PRIMARY}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_CUT_PRIMARY}, TOPIC_DPL);
break;
}
case EV_TC_CUT_BACKUP:
{
- sEventBroker->post(Event{EV_CUT_BACKUP}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_CUT_BACKUP}, TOPIC_DPL);
break;
}
case EV_TC_TEST_CUTTER_PRIMARY:
{
- sEventBroker->post(Event{EV_TEST_CUTTER_PRIMARY}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_TEST_CUTTER_PRIMARY}, TOPIC_DPL);
break;
}
case EV_TC_TEST_CUTTER_BACKUP:
{
- sEventBroker->post(Event{EV_TEST_CUTTER_BACKUP}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_TEST_CUTTER_BACKUP}, TOPIC_DPL);
break;
}
case EV_TC_RESET_SERVO:
{
- sEventBroker->post(Event{EV_RESET_SERVO}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_RESET_SERVO}, TOPIC_DPL);
break;
}
case EV_TC_WIGGLE_SERVO:
{
- sEventBroker->post(Event{EV_WIGGLE_SERVO}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_WIGGLE_SERVO}, TOPIC_DPL);
break;
}
case EV_TC_CLOSE_LOG:
@@ -468,7 +468,7 @@ State FlightModeManager::state_flying(const Event& ev)
{
// Open nosecone command sent by GS in case of problems
// during flight
- sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DPL);
break;
}
case EV_TC_END_MISSION:
@@ -533,7 +533,7 @@ State FlightModeManager::state_drogueDescent(const Event& ev)
case EV_ENTRY: /* Executed everytime state is entered */
{
// Open nosecone
- sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DPL);
logState(FMMState::DROGUE_DESCENT);
TRACE("[FMM] Entering drogueDescent\n");
@@ -573,7 +573,7 @@ State FlightModeManager::state_terminalDescent(const Event& ev)
{
sEventBroker->post({EV_DPL_ALTITUDE}, TOPIC_FLIGHT_EVENTS);
- sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DPL);
logState(FMMState::TERMINAL_DESCENT);
@@ -590,7 +590,7 @@ State FlightModeManager::state_terminalDescent(const Event& ev)
}
case EV_TC_CUT_DROGUE: // if you want to repeat cutting
{
- sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DEPLOYMENT);
+ sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DPL);
break;
}
default: /* If an event is not handled here, try with super-state */
diff --git a/src/boards/DeathStack/LoggerService/FlightStatsRecorder.cpp b/src/boards/DeathStack/LoggerService/FlightStatsRecorder.cpp
index 16332db3367ffc838904debf70bb4bb91dfefae6..19d777b702eb8854aec748a8c8a9a727528fb4f1 100644
--- a/src/boards/DeathStack/LoggerService/FlightStatsRecorder.cpp
+++ b/src/boards/DeathStack/LoggerService/FlightStatsRecorder.cpp
@@ -36,13 +36,13 @@ FlightStatsRecorder::FlightStatsRecorder()
: FSM(&FlightStatsRecorder::state_idle)
{
sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS);
- sEventBroker->subscribe(this, TOPIC_DEPLOYMENT);
+ sEventBroker->subscribe(this, TOPIC_DPL);
sEventBroker->subscribe(this, TOPIC_STATS);
}
FlightStatsRecorder::~FlightStatsRecorder() { sEventBroker->unsubscribe(this); }
-void FlightStatsRecorder::update(const KalmanState& t)
+void FlightStatsRecorder::update(const ADAKalmanState& t)
{
switch (state)
{
@@ -64,7 +64,7 @@ void FlightStatsRecorder::update(const KalmanState& t)
}
}
-void FlightStatsRecorder::update(const CurrentSenseData& t)
+void FlightStatsRecorder::update(const CurrentSenseDataWrapper& t)
{
switch (state)
{
@@ -86,13 +86,13 @@ void FlightStatsRecorder::update(const ADAData& t)
{
case State::LIFTOFF:
{
- if (t.acc_vert_speed > liftoff_stats.vert_speed_max)
+ /*if (t.acc_vert_speed > liftoff_stats.vert_speed_max)
{
liftoff_stats.vert_speed_max = t.acc_vert_speed;
liftoff_stats.T_max_speed =
static_cast<uint32_t>(miosix::getTick());
liftoff_stats.altitude_max_speed = t.msl_altitude;
- }
+ }*/
break;
}
case State::ASCENDING:
@@ -138,6 +138,8 @@ void FlightStatsRecorder::update(const AD7994WrapperData& t)
break;
}
}
+
+/*
void FlightStatsRecorder::update(const MPU9250Data& t)
{
switch (state)
@@ -174,7 +176,9 @@ void FlightStatsRecorder::update(const MPU9250Data& t)
break;
}
}
+*/
+/*
void FlightStatsRecorder::update(const PiksiData& t)
{
switch (state)
@@ -195,6 +199,7 @@ void FlightStatsRecorder::update(const PiksiData& t)
break;
}
}
+*/
void FlightStatsRecorder::state_idle(const Event& ev)
{
@@ -250,7 +255,7 @@ void FlightStatsRecorder::state_testing_cutters(const Event& ev)
ev_timeout_id =
sEventBroker
->postDelayed<FlightStatsConfig::TIMEOUT_CUTTER_TEST_STATS>(
- {EV_FLIGHTSTATS_TIMEOUT}, TOPIC_STATS);
+ {EV_STATS_TIMEOUT}, TOPIC_STATS);
StackLogger::getInstance()->updateStack(THID_STATS_FSM);
TRACE("[FlightStats] Entering CUTTER_TEST state\n");
@@ -269,7 +274,7 @@ void FlightStatsRecorder::state_testing_cutters(const Event& ev)
TRACE("[FlightStats] Exiting CUTTER_TEST state\n");
break;
}
- case EV_FLIGHTSTATS_TIMEOUT:
+ case EV_STATS_TIMEOUT:
{
transition(&FlightStatsRecorder::state_idle);
break;
@@ -294,7 +299,7 @@ void FlightStatsRecorder::state_liftOff(const Event& ev)
ev_timeout_id =
sEventBroker
->postDelayed<FlightStatsConfig::TIMEOUT_LIFTOFF_STATS>(
- {EV_FLIGHTSTATS_TIMEOUT}, TOPIC_STATS);
+ {EV_STATS_TIMEOUT}, TOPIC_STATS);
// Save liftoff time
liftoff_stats.T_liftoff = static_cast<uint32_t>(miosix::getTick());
@@ -311,7 +316,7 @@ void FlightStatsRecorder::state_liftOff(const Event& ev)
sEventBroker->removeDelayed(ev_timeout_id);
break;
}
- case EV_FLIGHTSTATS_TIMEOUT:
+ case EV_STATS_TIMEOUT:
{
transition(&FlightStatsRecorder::state_ascending);
break;
@@ -353,10 +358,10 @@ void FlightStatsRecorder::state_ascending(const Event& ev)
ev_timeout_id =
sEventBroker
->postDelayed<FlightStatsConfig::TIMEOUT_APOGEE_STATS>(
- {EV_FLIGHTSTATS_TIMEOUT}, TOPIC_STATS);
+ {EV_STATS_TIMEOUT}, TOPIC_STATS);
break;
}
- case EV_FLIGHTSTATS_TIMEOUT:
+ case EV_STATS_TIMEOUT:
{
// Drogue deployment occurs just after apogee
transition(&FlightStatsRecorder::state_drogueDeployment);
@@ -383,7 +388,7 @@ void FlightStatsRecorder::state_drogueDeployment(const Event& ev)
ev_timeout_id =
sEventBroker
->postDelayed<FlightStatsConfig::TIMEOUT_DROGUE_DPL_STATS>(
- {EV_FLIGHTSTATS_TIMEOUT}, TOPIC_STATS);
+ {EV_STATS_TIMEOUT}, TOPIC_STATS);
StackLogger::getInstance()->updateStack(THID_STATS_FSM);
break;
@@ -397,7 +402,7 @@ void FlightStatsRecorder::state_drogueDeployment(const Event& ev)
sEventBroker->removeDelayed(ev_timeout_id);
break;
}
- case EV_FLIGHTSTATS_TIMEOUT:
+ case EV_STATS_TIMEOUT:
{
transition(&FlightStatsRecorder::state_idle);
break;
@@ -426,7 +431,7 @@ void FlightStatsRecorder::state_mainDeployment(const Event& ev)
ev_timeout_id =
sEventBroker
->postDelayed<FlightStatsConfig::TIMEOUT_MAIN_DPL_STATS>(
- {EV_FLIGHTSTATS_TIMEOUT}, TOPIC_STATS);
+ {EV_STATS_TIMEOUT}, TOPIC_STATS);
StackLogger::getInstance()->updateStack(THID_STATS_FSM);
break;
@@ -440,7 +445,7 @@ void FlightStatsRecorder::state_mainDeployment(const Event& ev)
sEventBroker->removeDelayed(ev_timeout_id);
break;
}
- case EV_FLIGHTSTATS_TIMEOUT:
+ case EV_STATS_TIMEOUT:
{
transition(&FlightStatsRecorder::state_idle);
break;
diff --git a/src/boards/DeathStack/LoggerService/FlightStatsRecorder.h b/src/boards/DeathStack/LoggerService/FlightStatsRecorder.h
index 979d75d67c6496f918fdc0a950529df0568d4826..2b35165c8b782712c5e5a4cc98fafdacbfa44726 100644
--- a/src/boards/DeathStack/LoggerService/FlightStatsRecorder.h
+++ b/src/boards/DeathStack/LoggerService/FlightStatsRecorder.h
@@ -29,10 +29,10 @@
#include "ADA/ADAStatus.h"
#include "SensorManager/Sensors/AD7994WrapperData.h"
-#include "SensorManager/Sensors/PiksiData.h"
+//#include "SensorManager/Sensors/PiksiData.h"
#include "configs/FlightStatsConfig.h"
#include "FlightStatsData.h"
-#include "sensors/MPU9250/MPU9250Data.h"
+//#include "sensors/MPU9250/MPU9250Data.h"
#include "SensorManager/Sensors/ADCWrapperData.h"
namespace DeathStackBoard
@@ -50,12 +50,12 @@ public:
FlightStatsRecorder();
~FlightStatsRecorder();
- void update(const KalmanState& t);
- void update(const CurrentSenseData& t);
+ void update(const ADAKalmanState& t);
+ void update(const CurrentSenseDataWrapper& t);
void update(const ADAData& t);
void update(const AD7994WrapperData& t);
- void update(const MPU9250Data& t);
- void update(const PiksiData& t);
+ //void update(const MPU9250Data& t);
+ //void update(const PiksiData& t);
// Wait for liftoff or deployment
void state_idle(const Event& ev);
diff --git a/src/boards/DeathStack/LoggerService/LoggerService.cpp b/src/boards/DeathStack/LoggerService/LoggerService.cpp
index 4bc1007ae2536b04aab9d71f0a836b936089075c..67f28d752c7ad40a9d715af89ada6664bb62b379 100644
--- a/src/boards/DeathStack/LoggerService/LoggerService.cpp
+++ b/src/boards/DeathStack/LoggerService/LoggerService.cpp
@@ -32,13 +32,14 @@
#include "SensorManager/SensorManagerData.h"
#include "SensorManager/Sensors/AD7994WrapperData.h"
#include "SensorManager/Sensors/ADCWrapperData.h"
-#include "SensorManager/Sensors/PiksiData.h"
+//#include "SensorManager/Sensors/PiksiData.h"
+#include "sensors/Sensor.h"
#include "drivers/canbus/CanUtils.h"
#include "drivers/mavlink/MavlinkStatus.h"
#include "scheduler/TaskSchedulerData.h"
-#include "sensors/ADIS16405/ADIS16405Data.h"
-#include "sensors/MPU9250/MPU9250Data.h"
+//#include "sensors/ADIS16405/ADIS16405Data.h"
+//#include "sensors/MPU9250/MPU9250Data.h"
namespace DeathStackBoard
{
@@ -233,7 +234,7 @@ LogResult LoggerService::log<DeploymentStatus>(const DeploymentStatus& t)
miosix::PauseKernelLock kLock;
tm_repository.dpl_tm.fsm_state = (uint8_t)t.state;
- tm_repository.dpl_tm.cutter_state = (uint8_t)t.cutter_status.state;
+ //tm_repository.dpl_tm.cutter_state = (uint8_t)t.cutter_status.state;
// HR TM
tm_repository.hr_tm.dpl_state = t.state;
@@ -268,7 +269,7 @@ LogResult LoggerService::log<TargetDeploymentAltitude>(
/* ADA kalman filter values */
template <>
-LogResult LoggerService::log<KalmanState>(const KalmanState& t)
+LogResult LoggerService::log<ADAKalmanState>(const ADAKalmanState& t)
{
{
miosix::PauseKernelLock kLock;
@@ -298,7 +299,7 @@ LogResult LoggerService::log<ADAData>(const ADAData& t)
tm_repository.hr_tm.msl_altitude = t.msl_altitude;
tm_repository.hr_tm.agl_altitude = t.dpl_altitude;
tm_repository.hr_tm.vert_speed = t.vert_speed;
- tm_repository.hr_tm.vert_speed_2 = t.acc_vert_speed;
+ //tm_repository.hr_tm.vert_speed_2 = t.acc_vert_speed;
}
flight_stats.update(t);
@@ -380,7 +381,7 @@ LogResult LoggerService::log<AD7994WrapperData>(const AD7994WrapperData& t)
/* Battery status, sampled by internal ADC */
template <>
-LogResult LoggerService::log<BatteryVoltageData>(const BatteryVoltageData& t)
+LogResult LoggerService::log<BatteryVoltageDataWrapper>(const BatteryVoltageDataWrapper& t)
{
tm_repository.adc_tm.battery_voltage = t.volt;
tm_repository.test_tm.battery_volt = t.volt;
@@ -390,7 +391,7 @@ LogResult LoggerService::log<BatteryVoltageData>(const BatteryVoltageData& t)
/* Current sense, sampled by internal ADC */
template <>
-LogResult LoggerService::log<CurrentSenseData>(const CurrentSenseData& t)
+LogResult LoggerService::log<CurrentSenseDataWrapper>(const CurrentSenseDataWrapper& t)
{
{
miosix::PauseKernelLock kLock;
@@ -410,12 +411,13 @@ LogResult LoggerService::log<MS5803Data>(const MS5803Data& t)
{
miosix::PauseKernelLock kLock;
- tm_repository.hr_tm.pressure_digi = t.pressure;
+ tm_repository.hr_tm.pressure_digi = t.press;
}
return logger.log(t);
}
/* ADIS imu */
+/*
template <>
LogResult LoggerService::log<ADIS16405Data>(const ADIS16405Data& t)
{
@@ -441,8 +443,10 @@ LogResult LoggerService::log<ADIS16405Data>(const ADIS16405Data& t)
return logger.log(t);
}
+*/
/* MPU imu */
+/*
template <>
LogResult LoggerService::log<MPU9250Data>(const MPU9250Data& t)
{
@@ -473,6 +477,7 @@ LogResult LoggerService::log<MPU9250Data>(const MPU9250Data& t)
flight_stats.update(t);
return logger.log(t);
}
+*/
/* LM75b temperature */
template <>
@@ -489,7 +494,7 @@ LogResult LoggerService::log<LM75BData>(const LM75BData& t)
}
/* GPS */
-template <>
+/*template <>
LogResult LoggerService::log<PiksiData>(const PiksiData& t)
{
{
@@ -520,6 +525,7 @@ LogResult LoggerService::log<PiksiData>(const PiksiData& t)
return logger.log(t);
}
+*/
template <>
LogResult LoggerService::log<TaskStatResult>(const TaskStatResult& t)
diff --git a/src/boards/DeathStack/LoggerService/LoggerService.h b/src/boards/DeathStack/LoggerService/LoggerService.h
index 4532a89dc0b7d5aa377a4be16ec8c8f35cef9afa..564625ab39caad2029282f0b42e06f83896087a6 100644
--- a/src/boards/DeathStack/LoggerService/LoggerService.h
+++ b/src/boards/DeathStack/LoggerService/LoggerService.h
@@ -37,13 +37,13 @@
#include "SensorManager/SensorManagerData.h"
#include "SensorManager/Sensors/AD7994WrapperData.h"
#include "SensorManager/Sensors/ADCWrapperData.h"
-#include "SensorManager/Sensors/PiksiData.h"
+//#include "SensorManager/Sensors/PiksiData.h"
#include "drivers/canbus/CanUtils.h"
#include "drivers/mavlink/MavlinkStatus.h"
#include "scheduler/TaskSchedulerData.h"
-#include "sensors/ADIS16405/ADIS16405Data.h"
-#include "sensors/MPU9250/MPU9250Data.h"
+//#include "sensors/ADIS16405/ADIS16405Data.h"
+//#include "sensors/MPU9250/MPU9250Data.h"
#include "sensors/MS580301BA07/MS580301BA07Data.h"
namespace DeathStackBoard
@@ -149,7 +149,7 @@ LogResult LoggerService::log<TargetDeploymentAltitude>(
/* ADA kalman filter values */
template <>
-LogResult LoggerService::log<KalmanState>(const KalmanState& t);
+LogResult LoggerService::log<ADAKalmanState>(const ADAKalmanState& t);
/* ADA kalman altitude values */
template <>
@@ -168,26 +168,26 @@ LogResult LoggerService::log<AD7994WrapperData>(const AD7994WrapperData& t);
/* Battery status, sampled by internal ADC */
template <>
-LogResult LoggerService::log<BatteryVoltageData>(const BatteryVoltageData& t);
+LogResult LoggerService::log<BatteryVoltageDataWrapper>(const BatteryVoltageDataWrapper& t);
/* Motor current sense, sampled by internal ADC */
template <>
-LogResult LoggerService::log<CurrentSenseData>(const CurrentSenseData& t);
+LogResult LoggerService::log<CurrentSenseDataWrapper>(const CurrentSenseDataWrapper& t);
template <>
LogResult LoggerService::log<MS5803Data>(const MS5803Data& t);
/* ADIS imu */
-template <>
-LogResult LoggerService::log<ADIS16405Data>(const ADIS16405Data& t);
+//template <>
+//LogResult LoggerService::log<ADIS16405Data>(const ADIS16405Data& t);
/* MPU imu */
-template <>
-LogResult LoggerService::log<MPU9250Data>(const MPU9250Data& t);
+//template <>
+//LogResult LoggerService::log<MPU9250Data>(const MPU9250Data& t);
/* GPS */
-template <>
-LogResult LoggerService::log<PiksiData>(const PiksiData& t);
+//template <>
+//LogResult LoggerService::log<PiksiData>(const PiksiData& t);
/* LM75b temperature */
template <>
diff --git a/src/boards/DeathStack/PinHandler/PinHandler.cpp b/src/boards/DeathStack/PinHandler/PinHandler.cpp
index fb62a9dde0239d4226d261652eec9fd5b2c5ef71..48d755aa8fc178291929800bade10eed0b3887e4 100644
--- a/src/boards/DeathStack/PinHandler/PinHandler.cpp
+++ b/src/boards/DeathStack/PinHandler/PinHandler.cpp
@@ -1,6 +1,6 @@
/*
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+ * Copyright (c) 2019-2021 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -24,8 +24,8 @@
#include "PinHandler.h"
#include <events/EventBroker.h>
#include <functional>
-#include "events/Events.h"
#include "LoggerService/LoggerService.h"
+#include "events/Events.h"
using std::bind;
@@ -45,9 +45,9 @@ PinHandler::PinHandler()
PinObserver::OnStateChangeCallback launch_statechange_cb =
bind(&PinHandler::onLaunchPinStateChange, this, _1, _2, _3);
- pin_obs.observePin(PORT_LAUNCH_PIN, NUM_LAUNCH_PIN, TRIGGER_LAUNCH_PIN,
- launch_transition_cb, THRESHOLD_LAUNCH_PIN,
- launch_statechange_cb);
+ pin_obs.observePin(launch_pin.getPort(), launch_pin.getNumber(),
+ TRIGGER_LAUNCH_PIN, launch_transition_cb,
+ THRESHOLD_LAUNCH_PIN, launch_statechange_cb);
// Noseconse pin callbacks registration
PinObserver::OnTransitionCallback nc_transition_cb =
@@ -56,9 +56,20 @@ PinHandler::PinHandler()
PinObserver::OnStateChangeCallback nc_statechange_cb =
bind(&PinHandler::onNCPinStateChange, this, _1, _2, _3);
- pin_obs.observePin(PORT_NC_DETACH_PIN, NUM_NC_DETACH_PIN,
+ pin_obs.observePin(nosecone_pin.getPort(), nosecone_pin.getNumber(),
TRIGGER_NC_DETACH_PIN, nc_transition_cb,
THRESHOLD_NC_DETACH_PIN, nc_statechange_cb);
+
+ // Dpl servo pin callbacks registration
+ PinObserver::OnTransitionCallback dpl_transition_cb =
+ bind(&PinHandler::onDPLServoPinTransition, this, _1, _2);
+
+ PinObserver::OnStateChangeCallback dpl_statechange_cb =
+ bind(&PinHandler::onDPLServoPinStateChange, this, _1, _2, _3);
+
+ pin_obs.observePin(dpl_servo_pin.getPort(), dpl_servo_pin.getNumber(),
+ TRIGGER_DPL_SERVO_PIN, dpl_transition_cb,
+ THRESHOLD_DPL_SERVO_PIN, dpl_statechange_cb);
}
void PinHandler::onLaunchPinTransition(unsigned int p, unsigned char n)
@@ -67,7 +78,9 @@ void PinHandler::onLaunchPinTransition(unsigned int p, unsigned char n)
UNUSED(n);
sEventBroker->post(Event{EV_UMBILICAL_DETACHED}, TOPIC_FLIGHT_EVENTS);
- status_pin_launch.last_detection_time = miosix::getTick();
+ TRACE("[PinHandler] Launch pin detached! \n");
+
+ status_pin_launch.last_detection_time = TimestampTimer::getTimestamp();
logger->log(status_pin_launch);
}
@@ -77,32 +90,76 @@ void PinHandler::onNCPinTransition(unsigned int p, unsigned char n)
UNUSED(n);
sEventBroker->post(Event{EV_NC_DETACHED}, TOPIC_FLIGHT_EVENTS);
- status_pin_nosecone.last_detection_time = miosix::getTick();
+ TRACE("[PinHandler] Nosecone detached! \n");
+
+ status_pin_nosecone.last_detection_time = TimestampTimer::getTimestamp();
logger->log(status_pin_nosecone);
}
+void PinHandler::onDPLServoPinTransition(unsigned int p, unsigned char n)
+{
+ UNUSED(p);
+ UNUSED(n);
+
+ TRACE("[PinHandler] Deployment servo actuated! \n");
+
+ // do not post any event, just log the timestamp
+ status_pin_dpl_servo.last_detection_time = TimestampTimer::getTimestamp();
+ logger->log(status_pin_dpl_servo);
+}
+
void PinHandler::onLaunchPinStateChange(unsigned int p, unsigned char n,
- int state)
+ int state)
{
UNUSED(p);
UNUSED(n);
status_pin_launch.state = (uint8_t)state;
- status_pin_launch.last_state_change = miosix::getTick();
+ status_pin_launch.last_state_change = TimestampTimer::getTimestamp();
status_pin_launch.num_state_changes += 1;
+
+ TRACE(
+ "[PinHandler] Launch pin state change at time %llu: new "
+ "state = %d \n",
+ status_pin_launch.last_state_change, status_pin_launch.state);
+
logger->log(status_pin_launch);
}
-void PinHandler::onNCPinStateChange(unsigned int p, unsigned char n,
- int state)
+void PinHandler::onNCPinStateChange(unsigned int p, unsigned char n, int state)
{
UNUSED(p);
UNUSED(n);
status_pin_nosecone.state = (uint8_t)state;
- status_pin_nosecone.last_state_change = miosix::getTick();
+ status_pin_nosecone.last_state_change = TimestampTimer::getTimestamp();
status_pin_nosecone.num_state_changes += 1;
+
+ TRACE(
+ "[PinHandler] Nosecone pin state change at time %llu: new state = %d "
+ "\n",
+ status_pin_nosecone.last_state_change, status_pin_nosecone.state);
+
logger->log(status_pin_nosecone);
}
+void PinHandler::onDPLServoPinStateChange(unsigned int p, unsigned char n,
+ int state)
+{
+ UNUSED(p);
+ UNUSED(n);
+
+ status_pin_dpl_servo.state = (uint8_t)state;
+ status_pin_dpl_servo.last_state_change = TimestampTimer::getTimestamp();
+ status_pin_dpl_servo.num_state_changes += 1;
+
+ TRACE(
+ "[PinHandler] Deployment servo pin state change at time %llu: "
+ "new "
+ "state = %d \n",
+ status_pin_dpl_servo.last_state_change, status_pin_dpl_servo.state);
+
+ logger->log(status_pin_dpl_servo);
+}
+
} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/PinHandler/PinHandler.h b/src/boards/DeathStack/PinHandler/PinHandler.h
index 4b6b132907d395c07d84ccb921da9819a1520a3e..65cb7f5a4471dddea2c59ba690488fa5215e7212 100644
--- a/src/boards/DeathStack/PinHandler/PinHandler.h
+++ b/src/boards/DeathStack/PinHandler/PinHandler.h
@@ -1,6 +1,6 @@
/*
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+ * Copyright (c) 2019-2021 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -24,16 +24,15 @@
#pragma once
#include <utils/PinObserver.h>
-#include "configs/PinObserverConfig.h"
#include "PinHandlerData.h"
+#include "configs/PinObserverConfig.h"
namespace DeathStackBoard
{
-//Forward dec
+// Forward dec
class LoggerService;
-
/**
* @brief This class contains the handlers for both the launch pin (umbilical)
* and the nosecone detachment pin.
@@ -49,19 +48,13 @@ public:
* @brief Starts the pin observer
*
*/
- bool start()
- {
- return pin_obs.start();
- }
+ bool start() { return pin_obs.start(); }
/**
* @brief Stops the pin observer
*
*/
- void stop()
- {
- pin_obs.stop();
- }
+ void stop() { pin_obs.stop(); }
/**
* @brief Function called by the pinobserver when a launch pin detachment is
@@ -81,14 +74,23 @@ public:
*/
void onNCPinTransition(unsigned int p, unsigned char n);
+ /**
+ * @brief Function called by the pinobserver when a deployment servo
+ * actuation is detected via the optical sensor.
+ *
+ * @param p
+ * @param n
+ */
+ void onDPLServoPinTransition(unsigned int p, unsigned char n);
void onLaunchPinStateChange(unsigned int p, unsigned char n, int state);
void onNCPinStateChange(unsigned int p, unsigned char n, int state);
-
+ void onDPLServoPinStateChange(unsigned int p, unsigned char n, int state);
private:
PinStatus status_pin_launch{ObservedPin::LAUNCH};
PinStatus status_pin_nosecone{ObservedPin::NOSECONE};
+ PinStatus status_pin_dpl_servo{ObservedPin::DPL_SERVO};
PinObserver pin_obs;
diff --git a/src/boards/DeathStack/PinHandler/PinHandlerData.h b/src/boards/DeathStack/PinHandler/PinHandlerData.h
index 2361c4a320cc75f6440add366992fdd7fe616a72..8552c91679ad7c9afb4591912f5b6f443f43910a 100644
--- a/src/boards/DeathStack/PinHandler/PinHandlerData.h
+++ b/src/boards/DeathStack/PinHandler/PinHandlerData.h
@@ -1,6 +1,6 @@
/*
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+ * Copyright (c) 2019-2021 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -21,7 +21,6 @@
* THE SOFTWARE.
*/
-
#pragma once
#include <cstdint>
@@ -32,8 +31,9 @@ namespace DeathStackBoard
enum class ObservedPin : uint8_t
{
- LAUNCH = 0,
- NOSECONE = 1
+ LAUNCH = 0,
+ NOSECONE = 1,
+ DPL_SERVO = 2
};
/**
@@ -44,15 +44,15 @@ struct PinStatus
{
ObservedPin pin;
- long long last_state_change = 0; // Last time the pin changed state
- uint8_t state = 0; // Current state of the pin
+ uint64_t last_state_change = 0; // Last time the pin changed state
+ uint8_t state = 0; // Current state of the pin
unsigned int num_state_changes = 0;
- long long last_detection_time = 0; // When a transition is detected
+ uint64_t last_detection_time = 0; // When a transition is detected
- PinStatus() {};
+ PinStatus(){};
PinStatus(ObservedPin pin) : pin(pin) {}
-
+
static std::string header()
{
return "pin,last_state_change,state,num_state_changes,last_detection_"
diff --git a/src/boards/DeathStack/SensorManager/SensorManager.cpp b/src/boards/DeathStack/SensorManager/SensorManager.cpp
index 4c1e0fd577edd36c16ef4277edb65734a1728a68..ae9f38d9253749f32a5fa337a9db347d0133c492 100644
--- a/src/boards/DeathStack/SensorManager/SensorManager.cpp
+++ b/src/boards/DeathStack/SensorManager/SensorManager.cpp
@@ -20,37 +20,26 @@
* THE SOFTWARE.
*/
-#include <stdexcept>
-
#include "SensorManager.h"
-#include "System/StackLogger.h"
-#include "events/Events.h"
-#include "events/Topics.h"
-#include "Sensors/Test/TestSensor.h"
-#include "events/EventBroker.h"
-
#include <math/Stats.h>
+
#include <iostream>
+#include <stdexcept>
#include "SensorManagerData.h"
-#include "Sensors/AD7994Wrapper.h"
-#include "Sensors/ADCWrapper.h"
-#include "Sensors/PiksiData.h"
-#include "drivers/piksi/piksi.h"
-#include "sensors/ADIS16405/ADIS16405.h"
-#include "sensors/LM75B.h"
-#include "sensors/MPU9250/MPU9250.h"
-#include "sensors/MPU9250/MPU9250Data.h"
-#include "sensors/MS580301BA07/MS580301BA07.h"
+#include "System/StackLogger.h"
+#include "events/EventBroker.h"
+#include "events/Events.h"
+#include "events/Topics.h"
#ifdef USE_MOCK_SENSORS
+#include "Sensors/Test/TestSensor.h"
#include "Sensors/Test/MockGPS.h"
#include "Sensors/Test/MockPressureSensor.h"
#endif
-#include "Debug.h"
-
+#include "Common.h"
#include "interfaces-impl/hwmapping.h"
using miosix::FastMutex;
@@ -61,9 +50,9 @@ using namespace miosix;
namespace DeathStackBoard
{
-SensorManager::SensorManager(ADAController* ada_controller)
+SensorManager::SensorManager()
: FSM(&SensorManager::stateIdle), scheduler(),
- logger(*LoggerService::getInstance()), ada_controller(ada_controller)
+ logger(*LoggerService::getInstance())
{
sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS);
sEventBroker->subscribe(this, TOPIC_TC);
@@ -71,7 +60,6 @@ SensorManager::SensorManager(ADAController* ada_controller)
memset(&sensor_status, 0, sizeof(sensor_status));
initSensors();
- initSamplers();
initScheduler();
}
@@ -95,146 +83,17 @@ bool SensorManager::start()
void SensorManager::initSensors()
{
- i2c1::init();
- spiMPU9250::init();
- spiMS5803::init();
-
- // Instantiation
- adc_ad7994 = new AD7994Wrapper(sensors::ad7994::addr, AD7994_V_REF);
- temp_lm75b_analog = new LM75BType(sensors::lm75b_analog::addr);
- temp_lm75b_imu = new LM75BType(sensors::lm75b_imu::addr);
- pressure_ms5803 = new MS580301BA07Type();
-
- imu_mpu9250 =
- new MPU9250Type(MPU9250Type::ACC_FS_16G, MPU9250Type::GYRO_FS_2000);
-
- // imu_adis16405 = new ADIS16405Type(ADIS16405Type::GYRO_FS_300);
- adc_internal = new ADCWrapper();
-
- piksi = new Piksi("/dev/gps");
#ifdef USE_MOCK_SENSORS
mock_pressure_sensor = new MockPressureSensor();
mock_gps = new MockGPS();
#endif
- // Some sensors dont have init or self tests
- 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
- 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();
- sensor_status.current_sensor = adc_internal->getCurrentSensorPtr()->init();
-
- status.sensor_status = sensor_status.toNumeric();
-
TRACE("[SM] Sensor init done\n");
}
-void SensorManager::initSamplers()
-{
- sampler_20hz_simple.AddSensor(adc_internal->getBatterySensorPtr());
- sampler_20hz_simple.AddSensor(adc_ad7994);
- sampler_20hz_simple.AddSensor(adc_internal->getCurrentSensorPtr());
-
- sampler_20hz_simple.AddSensor(temp_lm75b_imu);
- sampler_20hz_simple.AddSensor(temp_lm75b_analog);
-
- sampler_50hz_simple.AddSensor(pressure_ms5803);
-
- sampler_250hz_simple.AddSensor(imu_mpu9250);
-
- // Piksi does not inherit from Sensor, so we sample it in a different way
-
- TRACE("[SM] Sampler init done\n");
-}
-
void SensorManager::initScheduler()
{
- /*
- * std::bind syntax:
- * std::bind(&MyClass::someFunction, &myclass_instance, [someFunction args])
- */
- long long start_time = miosix::getTick() + 10;
-
- // 250 Hz sensor sampler
- std::function<void()> simple_250hz_callback =
- std::bind(&SensorManager::onSimple250HZCallback, this);
- std::function<void()> simple_250hz_sampler =
- std::bind(&SimpleSensorSampler::UpdateAndCallback,
- &sampler_250hz_simple, simple_250hz_callback);
-
- scheduler.add(simple_250hz_sampler, 2,
- static_cast<uint8_t>(SensorSamplerId::SIMPLE_250HZ),
- start_time);
-
- // 100 Hz Calback ( MPU Magnetometer )
- std::function<void()> simple_100hz_callback =
- std::bind(&SensorManager::onSimple100HZCallback, this);
-
- scheduler.add(simple_100hz_callback, 10,
- static_cast<uint8_t>(SensorSamplerId::MPU_MAGN_100HZ),
- start_time);
-
- // 50 Hz sensor sampler
- std::function<void()> simple_50hz_callback =
- std::bind(&SensorManager::onSimple50HZCallback, this);
- std::function<void()> simple_50hz_sampler =
- std::bind(&SimpleSensorSampler::UpdateAndCallback, &sampler_50hz_simple,
- simple_50hz_callback);
-
- scheduler.add(simple_50hz_sampler, 20,
- static_cast<uint8_t>(SensorSamplerId::SIMPLE_50HZ),
- start_time);
-
- // 20 Hz sensor sampler
- std::function<void()> simple_20hz_callback =
- std::bind(&SensorManager::onSimple20HZCallback, this);
- std::function<void()> simple_20hz_sampler =
- std::bind(&SimpleSensorSampler::UpdateAndCallback, &sampler_20hz_simple,
- simple_20hz_callback);
-
- scheduler.add(simple_20hz_sampler, 50,
- static_cast<uint8_t>(SensorSamplerId::SIMPLE_20HZ),
- start_time);
-
- // Lambda expression to collect data from GPS at 10 Hz
- std::function<void()> gps_callback =
- std::bind(&SensorManager::onGPSCallback, this);
-
- scheduler.add(gps_callback, 100, static_cast<uint8_t>(SensorSamplerId::GPS),
- start_time);
-
- // Lambda expression callback to log scheduler stats, at 1 Hz
- scheduler.add(
- [&]() {
- scheduler_stats = scheduler.getTaskStats();
-
- for (TaskStatResult stat : scheduler_stats)
- logger.log(stat);
-
- StackLogger::getInstance()->updateStack(THID_SENSOR_SAMPLER);
- },
- 1000, static_cast<uint8_t>(SensorSamplerId::STATS), start_time);
TRACE("[SM] Scheduler initialization complete\n");
}
@@ -246,7 +105,7 @@ void SensorManager::stateIdle(const Event& ev)
case EV_ENTRY:
enable_sensor_logging = false;
- status.timestamp = miosix::getTick();
+ status.timestamp = TimestampTimer::getTimestamp();
status.state = SensorManagerState::IDLE;
logger.log(status);
@@ -277,7 +136,7 @@ void SensorManager::stateLogging(const Event& ev)
{
case EV_ENTRY:
enable_sensor_logging = true;
- status.timestamp = miosix::getTick();
+ status.timestamp = TimestampTimer::getTimestamp();
status.state = SensorManagerState::LOGGING;
logger.log(status);
@@ -295,8 +154,8 @@ void SensorManager::stateLogging(const Event& ev)
// Signal to the mock pressure sensor that we have liftoff in order
// to start simulating flight pressures
case EV_LIFTOFF:
- mock_pressure_sensor->before_liftoff = false;
- mock_gps->before_liftoff = false;
+ mock_pressure_sensor->signalLiftoff();
+ mock_gps->signalLiftoff();
break;
#endif
// Go back to idle in both cases
@@ -312,26 +171,15 @@ void SensorManager::stateLogging(const Event& ev)
void SensorManager::onSimple20HZCallback()
{
- AD7994WrapperData* ad7994_data = adc_ad7994->getDataPtr();
- LM75BData temp_data;
- temp_data.timestamp = miosix::getTick();
- temp_data.temp_analog = temp_lm75b_analog->getTemp();
- temp_data.temp_imu = temp_lm75b_imu->getTemp();
#ifdef USE_MOCK_SENSORS
- ad7994_data->nxp_baro_pressure = mock_pressure_sensor->getPressure();
+ PressureData p_data = mock_pressure_sensor->getLastSample();
#endif
if (enable_sensor_logging)
{
- logger.log(*(adc_internal->getBatterySensorPtr()->getBatteryDataPtr()));
- logger.log(*(adc_internal->getCurrentSensorPtr()->getCurrentDataPtr()));
- logger.log(*(ad7994_data));
-
- logger.log(temp_data);
+ // logger.log(...);
}
-
- ada_controller->updateBaro(ad7994_data->nxp_baro_pressure);
}
void SensorManager::onSimple50HZCallback()
@@ -340,79 +188,28 @@ void SensorManager::onSimple50HZCallback()
{
// Since sampling both temps & pressure on the ms5803 takes two calls of
// onSimpleUpdate(), log only once every 2
- if (pressure_ms5803->getState() ==
+ /*if (pressure_ms5803->getState() ==
MS580301BA07Type::STATE_SAMPLED_PRESSURE)
{
logger.log(pressure_ms5803->getData());
- }
+ }*/
}
}
-void SensorManager::onSimple100HZCallback()
-{
- imu_mpu9250->updateMagneto();
-
- // Don't log here, magnetometer data will be logged in the 250 hz task with
- // the accel & gyro data.
-}
-
-void SensorManager::onSimple250HZCallback()
-{
- MPU9250Data mpu9255_data{miosix::getTick(), *(imu_mpu9250->accelDataPtr()),
- *(imu_mpu9250->gyroDataPtr()),
- *(imu_mpu9250->compassDataPtr()),
- *(imu_mpu9250->tempDataPtr())};
-
- // Correct bias
- mpu9255_data.accel = Vec3(mpu9255_data.accel.getX() + OFFSET_MPU_ACC_X,
- mpu9255_data.accel.getY() + OFFSET_MPU_ACC_Y,
- mpu9255_data.accel.getZ() + OFFSET_MPU_ACC_Z);
-
- if (enable_sensor_logging)
- {
- logger.log(mpu9255_data);
- }
+void SensorManager::onSimple100HZCallback() {}
- ada_controller->updateAcc(mpu9255_data.accel.getZ());
-}
+void SensorManager::onSimple250HZCallback() {}
void SensorManager::onGPSCallback()
{
- PiksiData data;
- memset(&data, 0, sizeof(data));
-
- try
- {
- data.gps_data = piksi->getGpsData();
-
- long long fix_age = getTick() - data.gps_data.timestamp;
- // We have fix if the GPS sample is not too old and the number of
- // satellites is at least 4
- data.fix =
- fix_age <= MAX_GPS_FIX_AGE && data.gps_data.numSatellites >= 4;
-
- last_gps_timestamp = data.gps_data.timestamp;
- }
- catch (std::runtime_error rterr)
- {
- data.gps_data.timestamp = miosix::getTick();
- data.fix = false;
- }
#ifdef USE_MOCK_SENSORS
- mock_gps->updateCoordinates();
- data.gps_data.timestamp = miosix::getTick();
- data.gps_data.latitude = mock_gps->lat;
- data.gps_data.longitude = mock_gps->lon;
- data.fix = mock_gps->fix;
+ GPSData data = mock_gps->getLastSample();
#endif
- ada_controller->updateGPS(data.gps_data.latitude, data.gps_data.longitude,
- data.fix);
-
if (enable_sensor_logging)
{
- logger.log(data);
+ // logger.log(data);
}
}
diff --git a/src/boards/DeathStack/SensorManager/SensorManager.h b/src/boards/DeathStack/SensorManager/SensorManager.h
index c16ad9f88a6823673e7287310626ba012c3f194e..bad5e4aacc983614c54ca81d9d130e8ccda08064 100644
--- a/src/boards/DeathStack/SensorManager/SensorManager.h
+++ b/src/boards/DeathStack/SensorManager/SensorManager.h
@@ -22,56 +22,31 @@
#pragma once
-#include <vector>
-#include "Common.h"
-
+#include <interfaces-impl/hwmapping.h>
#include <math/Stats.h>
#include <scheduler/TaskScheduler.h>
-#include <sensors/SensorSampling.h>
+#include <sensors/SensorSampler.h>
-#include "LoggerService/LoggerService.h"
-#include "configs/SensorManagerConfig.h"
-#include "events/FSM.h"
+#include <vector>
#include "ADA/ADAController.h"
+#include "Common.h"
+#include "LoggerService/LoggerService.h"
#include "SensorManagerData.h"
-
-#include <interfaces-impl/hwmapping.h>
+#include "configs/SensorManagerConfig.h"
+#include "events/FSM.h"
using miosix::PauseKernelLock;
using std::vector;
-// Forward declarations
-template <typename ProtocolSPI>
-class MPU9250;
-
-template <typename ProtocolSPI, typename RST>
-class ADIS16405;
-
-template <typename ProtocolI2C>
-class LM75B;
-
-template <typename ProtocolSPI>
-class MS580301BA07;
-
-class Piksi;
-
namespace DeathStackBoard
{
-class ADCWrapper;
-class AD7994Wrapper;
#ifdef USE_MOCK_SENSORS
class MockPressureSensor;
class MockGPS;
#endif
-// Type definitions
-typedef MPU9250<spiMPU9250> MPU9250Type;
-typedef ADIS16405<spiADIS16405, miosix::sensors::adis16405::rst> ADIS16405Type;
-typedef MS580301BA07<spiMS5803> MS580301BA07Type;
-
-typedef LM75B<i2c1> LM75BType;
/**
* The SensorManager class manages all the sensors connected to the Homeone
* Board.
@@ -85,7 +60,7 @@ typedef LM75B<i2c1> LM75BType;
class SensorManager : public FSM<SensorManager>
{
public:
- SensorManager(ADAController* ada);
+ SensorManager();
~SensorManager();
vector<TaskStatResult> getSchedulerStats() { return scheduler_stats; }
@@ -101,9 +76,9 @@ private:
void initSensors();
/**
- * Initialize the samplers.
+ * Adds all the SensorSamplers to the scheduler and begins sampling.
*/
- void initSamplers();
+ void initScheduler();
/**
* @brief Sensor manager state machine entry state
@@ -117,11 +92,6 @@ private:
*/
void stateLogging(const Event& ev);
- /**
- * Adds all the SensorSamplers to the scheduler and begins sampling.
- */
- void initScheduler();
-
/*
* Callbacks. These functions are called each time the corresponding
* SensorSampler has acquired new samples.
@@ -133,10 +103,10 @@ private:
* Simple, 20 Hz SensorSampler Callback.
* Called each time all the sensors in the 20hz sampler have been sampled
*/
- void onSimple20HZCallback(); // ADCs
- void onSimple50HZCallback(); // MS5803
- void onSimple100HZCallback(); // Mpu magneto
- void onSimple250HZCallback(); // Mpu accel & gyro
+ void onSimple20HZCallback(); // ADCs
+ void onSimple50HZCallback(); // MS5803
+ void onSimple100HZCallback(); // Mpu magneto
+ void onSimple250HZCallback(); // Mpu accel & gyro
void onGPSCallback();
@@ -146,30 +116,14 @@ private:
bool enable_sensor_logging = false;
- // Sensor samplers
- SimpleSensorSampler sampler_20hz_simple;
- SimpleSensorSampler sampler_50hz_simple;
- SimpleSensorSampler sampler_250hz_simple;
-
// Sensors
- AD7994Wrapper* adc_ad7994;
- MPU9250Type* imu_mpu9250;
- ADIS16405Type* imu_adis16405;
- LM75BType* temp_lm75b_imu;
- LM75BType* temp_lm75b_analog;
- ADCWrapper* adc_internal;
- MS580301BA07Type* pressure_ms5803;
#ifdef USE_MOCK_SENSORS
MockPressureSensor* mock_pressure_sensor;
MockGPS* mock_gps;
#endif
- Piksi* piksi;
long long last_gps_timestamp = 0;
- // ADA
- ADAController* ada_controller;
-
// Stats & status
vector<TaskStatResult> scheduler_stats;
diff --git a/src/boards/DeathStack/SensorManager/Sensors/ADCWrapper.h b/src/boards/DeathStack/SensorManager/Sensors/ADCWrapper.h
index 4f08c09892ed0b67049708de629cd0e558f7dfb5..be5ef139e4085d56174a13a14dc026af65e3306d 100644
--- a/src/boards/DeathStack/SensorManager/Sensors/ADCWrapper.h
+++ b/src/boards/DeathStack/SensorManager/Sensors/ADCWrapper.h
@@ -73,7 +73,7 @@ public:
/**
* @brief Returns a pointer to the last conveted current sense value
*/
- CurrentSenseData* getCurrentDataPtr() { return ¤t_data; }
+ CurrentSenseDataWrapper* getCurrentDataPtr() { return ¤t_data; }
private:
float adcToCurrent(uint16_t adc_in) { return (adc_in - 107) / 23.8f; }
@@ -85,7 +85,7 @@ public:
static_cast<adc_t::Channel>(ADC_CURRENT_SENSE_2_CHANNEL);
ADCWrapper& parent;
- CurrentSenseData current_data;
+ CurrentSenseDataWrapper current_data;
};
/**
@@ -118,14 +118,14 @@ public:
/**
* @brief Returns the pointer to the result of the last conversion.
*/
- BatteryVoltageData* getBatteryDataPtr() { return &battery_data; }
+ BatteryVoltageDataWrapper* getBatteryDataPtr() { return &battery_data; }
private:
const adc_t::Channel BATTERY_VOLT_CHANNEL =
static_cast<adc_t::Channel>(ADC_BATTERY_VOLTAGE_CHANNEL);
ADCWrapper& parent;
- BatteryVoltageData battery_data;
+ BatteryVoltageDataWrapper battery_data;
};
BatterySensor* getBatterySensorPtr() { return &battery_sensor; }
diff --git a/src/boards/DeathStack/SensorManager/Sensors/ADCWrapperData.h b/src/boards/DeathStack/SensorManager/Sensors/ADCWrapperData.h
index 6693a3da7203dc594899fdd260d459646b1d48b9..dc90959cde16df2a4402d78429d0968594a9c6ca 100644
--- a/src/boards/DeathStack/SensorManager/Sensors/ADCWrapperData.h
+++ b/src/boards/DeathStack/SensorManager/Sensors/ADCWrapperData.h
@@ -26,7 +26,7 @@
#include <cstdint>
#include <ostream>
-struct BatteryVoltageData
+struct BatteryVoltageDataWrapper
{
long long timestamp;
@@ -41,7 +41,7 @@ struct BatteryVoltageData
}
};
-struct CurrentSenseData
+struct CurrentSenseDataWrapper
{
long long timestamp;
uint16_t raw_value_1;
diff --git a/src/boards/DeathStack/SensorManager/Sensors/PiksiData.h b/src/boards/DeathStack/SensorManager/Sensors/PiksiData.h
index 14ba778c96c8f5a384877b84666215ee1d994b22..c8f4377d6ba4b4c41e1bd9570f7dfb94cb92e803 100644
--- a/src/boards/DeathStack/SensorManager/Sensors/PiksiData.h
+++ b/src/boards/DeathStack/SensorManager/Sensors/PiksiData.h
@@ -30,7 +30,7 @@ namespace DeathStackBoard
{
struct PiksiData
{
- GPSData gps_data;
+ //GPSData gps_data;
bool fix = false;
static std::string header()
@@ -40,11 +40,11 @@ struct PiksiData
void print(std::ostream& os) const
{
- os << gps_data.timestamp << "," << gps_data.latitude << ","
+ /*os << gps_data.timestamp << "," << gps_data.latitude << ","
<< gps_data.longitude << "," << gps_data.height << ","
<< gps_data.velocityNorth << "," << gps_data.velocityEast << ","
<< gps_data.velocityDown << "," << gps_data.speed << ","
- << gps_data.numSatellites << "," << (int)fix << "\n";
+ << gps_data.numSatellites << "," << (int)fix << "\n";*/
}
};
} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/SensorManager/Sensors/Test/MockGPS.h b/src/boards/DeathStack/SensorManager/Sensors/Test/MockGPS.h
index 1f5107b2d32223780743f424c72e98960e6e4d92..078f84ba17fae536996e8cacac6c0da76b35c475 100644
--- a/src/boards/DeathStack/SensorManager/Sensors/Test/MockGPS.h
+++ b/src/boards/DeathStack/SensorManager/Sensors/Test/MockGPS.h
@@ -21,15 +21,22 @@
*/
#include <tests/mock_sensors/test-mock-data.h>
+#include "TimestampTimer.h"
+#include "sensors/Sensor.h"
namespace DeathStackBoard
{
-class MockGPS
+
+class MockGPS : public Sensor<GPSData>
{
public:
- MockGPS(){}
+ MockGPS() { }
+
+ bool init() override { return true; }
+
+ bool selfTest() override { return true; }
- bool updateCoordinates()
+ GPSData sampleImpl() override
{
// if (inside_lha)
// {
@@ -41,26 +48,31 @@ public:
// lat = lat_outside;
// lon = lon_outside;
// }
+
+ GPSData data;
+
+ data.gps_timestamp = TimestampTimer::getTimestamp();
+ data.fix = true;
+
if (before_liftoff)
{
- lat = SIMULATED_LAT[0];
- lon = SIMULATED_LON[0];
+ data.latitude = SIMULATED_LAT[0];
+ data.longitude = SIMULATED_LON[0];
+ //data.height = SIMULATED_HEIGHT[0];
}
else if (i < GPS_DATA_SIZE)
{
- lat = SIMULATED_LAT[i];
- lon = SIMULATED_LON[i];
+ data.latitude = SIMULATED_LAT[i];
+ data.longitude = SIMULATED_LON[i];
+ //data.height = SIMULATED_HEIGHT[i];
i++;
}
- return true;
- }
- volatile bool before_liftoff = true;
+ return data;
+ }
- bool inside_lha = true;
+ void signalLiftoff() { before_liftoff = false; }
- double lat, lon;
- bool fix = true;
private:
// Set of coordinates inside the Launch Hazard Area
// const double lat_inside = 41.807487124105;
@@ -70,6 +82,9 @@ private:
// const double lat_outside = 41.840794;
// const double lon_outside = 14.003920;
+ volatile bool before_liftoff = true;
+ bool inside_lha = true;
+
unsigned int i = 0;
};
} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/SensorManager/Sensors/Test/MockPressureSensor.h b/src/boards/DeathStack/SensorManager/Sensors/Test/MockPressureSensor.h
index 4b95eedc4fc29a76c7b090c6de271a652a26d2bc..7d2f8c83fe9e2f9c81cfef2abe517bbeca51b6d7 100644
--- a/src/boards/DeathStack/SensorManager/Sensors/Test/MockPressureSensor.h
+++ b/src/boards/DeathStack/SensorManager/Sensors/Test/MockPressureSensor.h
@@ -22,38 +22,50 @@
#include <Common.h>
#include <sensors/Sensor.h>
-// #include <tests/catch/ada/test-ada-data.h>
-#include <tests/mock_sensors/test-mock-data.h>
+#include <tests/catch/ada/ada_kalman_p/test-ada-data.h>
+//#include <tests/mock_sensors/test-mock-data.h>
#include <random>
namespace DeathStackBoard
{
-class MockPressureSensor
+
+class MockPressureSensor : public Sensor<PressureData>
{
public:
- float getPressure()
+ MockPressureSensor() {}
+
+ bool init() override { return true; }
+
+ bool selfTest() override { return true; }
+
+ PressureData sampleImpl() override
{
+ float press = 0.0;
+
if (before_liftoff)
{
- return addNoise(SIMULATED_PRESSURE[0]);
+ press = addNoise(SIMULATED_PRESSURE[0]);
}
else
{
if (i < PRESSURE_DATA_SIZE)
{
- return addNoise(SIMULATED_PRESSURE[i++]);
+ press = addNoise(SIMULATED_PRESSURE[i++]);
}
else
{
- return addNoise(SIMULATED_PRESSURE[PRESSURE_DATA_SIZE - 1]);
+ press = addNoise(SIMULATED_PRESSURE[PRESSURE_DATA_SIZE - 1]);
}
}
+
+ return PressureData{TimestampTimer::getTimestamp(), press};
}
- volatile bool before_liftoff = true;
+ void signalLiftoff() { before_liftoff = false; }
private:
- volatile unsigned int i = 0; // Last index
+ volatile bool before_liftoff = true;
+ volatile unsigned int i = 0; // Last index
std::default_random_engine generator{1234567};
std::normal_distribution<float> distribution{0.0f, 5.0f};
@@ -62,6 +74,7 @@ private:
return quantization(sample + distribution(generator));
}
- float quantization(float sample) { return round(sample / 30) * 30; }
+ float quantization(float sample) { return round(sample / 30.0) * 30.0; }
};
+
} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/SensorManager/Sensors/Test/TestSensor.h b/src/boards/DeathStack/SensorManager/Sensors/Test/TestSensor.h
index 8ab13bf657b49e0dbbec28ea108b49fd89c30e57..42629515079f3dcc085d1b374dbca4b8c3b64a19 100644
--- a/src/boards/DeathStack/SensorManager/Sensors/Test/TestSensor.h
+++ b/src/boards/DeathStack/SensorManager/Sensors/Test/TestSensor.h
@@ -23,33 +23,45 @@
#define SRC_SHARED_BOARDS_HOMEONE_SENSORMANAGER_TESTSENSOR_H
#include <cmath>
+
#include "Common.h"
#include "sensors/Sensor.h"
using miosix::getTick;
using miosix::TICK_FREQ;
-class TestSensor : public Sensor
+struct TestSensorData : public TimestampData
+{
+ float value;
+
+ TestSensorData()
+ : TimestampData{0}, value(0.0)
+ {
+ }
+
+ TestSensorData(uint64_t timestamp, float value)
+ : TimestampData{timestamp}, value(value)
+ {
+ }
+};
+
+class TestSensor : public Sensor<TestSensorData>
{
public:
- TestSensor() : mLastSample(0) {}
+ TestSensor() {}
virtual ~TestSensor() {}
bool init() { return true; }
- bool onSimpleUpdate()
- {
- // printf("onSimpleUpdate\n");
- mLastSample = 10 * sin(PI * static_cast<float>(getTick()) /
- static_cast<float>(TICK_FREQ));
- return true;
- }
bool selfTest() { return true; }
- float* testDataPtr() { return &mLastSample; }
+ TestSensorData sampleImpl()
+ {
+ float v = 10 * sin(PI * static_cast<float>(getTick()) /
+ static_cast<float>(TICK_FREQ));
-private:
- float mLastSample;
+ return TestSensorData(static_cast<uint64_t>(getTick()), v);
+ }
};
#endif /* SRC_SHARED_BOARDS_HOMEONE_SENSORMANAGER_TESTSENSOR_H */
diff --git a/src/boards/DeathStack/ServoInterface.h b/src/boards/DeathStack/ServoInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..cdf6ddaf759092eafd091243d5c41b0765ad87c8
--- /dev/null
+++ b/src/boards/DeathStack/ServoInterface.h
@@ -0,0 +1,153 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Vincenzo Santomarco
+ *
+ * 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
+
+namespace DeathStackBoard
+{
+
+/**
+ * @brief Class for interfacing with 180° servo motors, works in degrees.
+ *
+ * This class provides all the methods for enabling/disabling servo
+ * communication as well as methods for testing it (like reset, setMaxPosition
+ * and selftest).
+ * The function set(float) is used to send data to the servo and calls, in
+ * sequence, the preprocessPosition() and the setPosition() functions. This
+ * function must not be overridden and all the logic of preprocessing and
+ * sending position to the actual servo must be implemented via those two
+ * methods.
+ * */
+class ServoInterface
+{
+public:
+ /**
+ * @brief Class constructor.
+ *
+ * @param minPosition The minimum position possible for the Servo
+ * @param maxPosition The maximum position possible for the Servo
+ * */
+ ServoInterface(float minPosition, float maxPosition)
+ : MIN_POS(minPosition), MAX_POS(maxPosition), RESET_POS(minPosition)
+ {
+ }
+
+ /**
+ * @brief Class constructor.
+ *
+ * @param minPosition The minimum position possible for the Servo
+ * @param maxPosition The maximum position possible for the Servo
+ * @param resetPosition The reset position for the Servo
+ * */
+ ServoInterface(float minPosition, float maxPosition, float resetPosition)
+ : MIN_POS(minPosition), MAX_POS(maxPosition), RESET_POS(resetPosition)
+ {
+ }
+
+ /**
+ * @brief Enables the communication with the servo and sets it to its reset
+ * position.
+ * */
+ virtual void enable() = 0;
+
+ /**
+ * @brief Disables the communication with the servo.
+ * */
+ virtual void disable() = 0;
+
+ /**
+ * @brief Sends the given input to the Servo. Before sending data the input
+ * is preprocessed in order to have a physical consistent position to send
+ * to the servo. Do not override this method, but use @see{setPosition} and
+ * @see{preprocessPosition}.
+ *
+ * @param angle The input to be sent to the Servo
+ * */
+ void set(float angle) { setPosition(preprocessPosition(angle)); }
+
+ /**
+ * @brief Sends the Servo the highest input possible
+ * */
+ void setMaxPosition() { setPosition(MAX_POS); }
+
+ /**
+ * @brief Sends the Servo the lowest input possible
+ * */
+ void setMinPosition() { setPosition(MIN_POS); }
+
+ /**
+ * @brief Sets servo to its reset position
+ * */
+ void reset() { setPosition(RESET_POS); }
+
+ /**
+ * @return current actuator position (in degrees)
+ */
+ float getCurrentPosition()
+ {
+ return currentPosition;
+ }
+
+ virtual void selfTest() = 0;
+
+ const float MIN_POS = 0;
+ const float MAX_POS = 0;
+ const float RESET_POS = 0;
+
+protected:
+ /**
+ * @brief Sends the data to the servo
+ *
+ * @param angle Data to be sent to servo
+ * */
+ virtual void setPosition(float angle) = 0;
+
+ /**
+ * @brief Applies any transformation needed to the data before actually
+ * sending it to the servo
+ *
+ * @param angle Non normalized input position
+ *
+ * @returns Normalized input position
+ * */
+ virtual float preprocessPosition(float angle)
+ {
+ if (angle > MAX_POS)
+ {
+ angle = MAX_POS;
+ }
+ else if (angle < MIN_POS)
+ {
+ angle = MIN_POS;
+ }
+
+ return angle;
+ };
+
+ /**
+ * @brief Actuator's current position.
+ */
+ float currentPosition = 0;
+};
+
+} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/TMTCManager/TMBuilder.h b/src/boards/DeathStack/TMTCManager/TMBuilder.h
index 490191d521ee03dcfd4ab2382d97819737fc9373..8ad7603c65b170a7e18bded12f94f12270173836 100644
--- a/src/boards/DeathStack/TMTCManager/TMBuilder.h
+++ b/src/boards/DeathStack/TMTCManager/TMBuilder.h
@@ -25,6 +25,7 @@
#include <Common.h>
#include <LoggerService/TmRepository.h>
#include <configs/TMTCConfig.h>
+
namespace DeathStackBoard
{
namespace TMBuilder
diff --git a/src/boards/DeathStack/TMTCManager/TMTCManager.cpp b/src/boards/DeathStack/TMTCManager/TMTCManager.cpp
index 2245c9635467d5a0d21ae579da51d2b8368ab17a..ae27db8463f8eaa954a06c2b562e23465b77e525 100644
--- a/src/boards/DeathStack/TMTCManager/TMTCManager.cpp
+++ b/src/boards/DeathStack/TMTCManager/TMTCManager.cpp
@@ -21,22 +21,23 @@
*/
#include "TMTCManager.h"
-#include <configs/TMTCConfig.h>
+
#include <events/Events.h>
#include <events/Topics.h>
-#include <drivers/Xbee/Xbee.h>
+
#include "TCHandler.h" // Real message handling is here
#include "XbeeInterrupt.h"
#include "bitpacking/hermes/HermesPackets.h"
+
namespace DeathStackBoard
{
-TMTCManager::TMTCManager() : FSM(&TMTCManager::stateGroundTM)
+TMTCManager::TMTCManager() : FSM(&TMTCManager::stateGroundTM), xbee_bus(SPI2)
{
- busSPI2::init();
enableXbeeInterrupt();
- device = new Xbee_t();
+ device = new Xbee::Xbee(xbee_bus, XbeeCS::getPin(), XbeeATTN::getPin(),
+ XbeeRST::getPin());
channel = new Mav(device,
&TCHandler::handleMavlinkMessage, // rcv function
TMTC_SLEEP_AFTER_SEND, MAV_OUT_BUFFER_MAX_AGE);
diff --git a/src/boards/DeathStack/TMTCManager/TMTCManager.h b/src/boards/DeathStack/TMTCManager/TMTCManager.h
index 6c5d04c77089cc7c40ec0168bf346188c9fcb99d..51445eb1f6f743d125171fd87ac36e9f2d6b909c 100644
--- a/src/boards/DeathStack/TMTCManager/TMTCManager.h
+++ b/src/boards/DeathStack/TMTCManager/TMTCManager.h
@@ -28,7 +28,8 @@
#include <drivers/mavlink/MavlinkDriver.h>
#include <LoggerService/LoggerService.h>
-#include <interfaces-impl/hwmapping.h>
+#include "configs/TMTCConfig.h"
+#include "drivers/Xbee/Xbee.h"
namespace DeathStackBoard
{
@@ -65,7 +66,7 @@ public:
}
private:
- Xbee_t* device;
+ Xbee::Xbee* device;
Mav* channel;
LoggerService& logger = *(LoggerService::getInstance());
@@ -78,6 +79,8 @@ private:
inline void packHRTelemetry(uint8_t* packet, unsigned int index);
inline void packLRTelemetry(uint8_t* packet);
+ SPIBus xbee_bus;
+
uint16_t lr_event_id = 0;
uint16_t hr_event_id = 0;
uint16_t test_tm_event_id = 0;
diff --git a/src/boards/DeathStack/configs/ADA_config.h b/src/boards/DeathStack/configs/ADAconfig.h
similarity index 55%
rename from src/boards/DeathStack/configs/ADA_config.h
rename to src/boards/DeathStack/configs/ADAconfig.h
index 0d8d9d3460748c818701f29667fe084d16b4c1a4..28aecfdb994b5b1afe45300bc2559eb4e54431ff 100644
--- a/src/boards/DeathStack/configs/ADA_config.h
+++ b/src/boards/DeathStack/configs/ADAconfig.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2018,2019 Skyward Experimental Rocketry
- * Authors: Luca Mozzarelli
+/* Copyright (c) 2018-2021 Skyward Experimental Rocketry
+ * Authors: Luca Mozzarelli, Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -21,14 +21,24 @@
*/
#pragma once
-#include "skyward-boardcore/libs/simple-template-matrix/matrix.h"
+
+#include <Eigen/Dense>
+
+#include "kalman/KalmanEigen.h"
namespace DeathStackBoard
{
+
+namespace ADAConfigs
+{
+
+static const unsigned int ADA_STACK_SIZE = 4096;
+static const unsigned int ADA_PRIORITY = 2; // high
+
// Number of consecutive samples with negative speed after which AD is triggered
constexpr unsigned int APOGEE_N_SAMPLES = 5;
-// Number of consecutive samples after which Deployment is triggered
+// Number of consecutive samples after which the main Deployment is triggered
constexpr unsigned int DEPLOYMENT_N_SAMPLES = 5;
// When the vertical speed is smaller than this value, apogee is detected.
@@ -37,11 +47,11 @@ constexpr unsigned int DEPLOYMENT_N_SAMPLES = 5;
constexpr float APOGEE_VERTICAL_SPEED_TARGET = 2.5;
// State timeouts
-static const unsigned int TIMEOUT_ADA_SHADOW_MODE = 6.5 * 1000; // ms
-static const unsigned int TIMEOUT_ADA_P_STABILIZATION = 5 * 1000; // ms
+static const unsigned int TIMEOUT_ADA_SHADOW_MODE = 6.5 * 1000; // ms
+static const unsigned int TIMEOUT_ADA_P_STABILIZATION = 5 * 1000; // ms
// Number of samples used to calibrate the kalman initial state
-static const unsigned int CALIBRATION_BARO_N_SAMPLES = 1200;
+static const unsigned int CALIBRATION_BARO_N_SAMPLES = 50; // 1200
static const unsigned int ACCELERATION_AVERAGE_N_SAMPLES = 25;
// Default reference values settings
@@ -54,8 +64,6 @@ static const float DEFAULT_MSL_TEMPERATURE = 288.15f;
static const float DEFAULT_MSL_PRESSURE = 101325.0f;
// Deployment altitude AGL
-// Set it under the ground level: don't deploy the Rogallo wing if we somehow
-// forget to set the deployment altitude via telecommand
static const float DEFAULT_DEPLOYMENT_ALTITUDE = -100;
// Do cut the drogue above this altitude
@@ -64,31 +72,63 @@ static const float MAX_DEPLOYMENT_ALTITUDE_MSL = 1800;
// ------ Kalman parameters ------
static const float SAMPLING_PERIOD = 1 / 20.0f; // In seconds
-// State 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
-static const MatrixBase<float, 3, 3> A_INIT(
- {1.0f, SAMPLING_PERIOD, 0.5f * SAMPLING_PERIOD* SAMPLING_PERIOD, 0.0f, 1.0f,
- SAMPLING_PERIOD, 0.0f, 0.0f, 1.0f});
+// Initialize the Kalman filter with a negative (pressure) acceleration in order
+// to make it more respondive during the propulsive phase
+static const float KALMAN_INITIAL_ACCELERATION = -500;
+
+// kalman dimensions
+static const uint8_t KALMAN_STATES_NUM = 3;
+static const uint8_t KALMAN_OUTPUTS_NUM = 1;
+
+using MatrixNN = Matrix<float, KALMAN_STATES_NUM, KALMAN_STATES_NUM>;
+using MatrixPN = Matrix<float, KALMAN_OUTPUTS_NUM, KALMAN_STATES_NUM>;
+using MatrixNP = Matrix<float, KALMAN_STATES_NUM, KALMAN_OUTPUTS_NUM>;
+using MatrixPP = Matrix<float, KALMAN_OUTPUTS_NUM, KALMAN_OUTPUTS_NUM>;
+using CVectorN = Matrix<float, KALMAN_STATES_NUM, 1>;
+using CVectorP = Matrix<float, KALMAN_OUTPUTS_NUM, 1>;
+
+// kalman matrices
+static const MatrixNN F_INIT =
+ (MatrixNN(KALMAN_STATES_NUM, KALMAN_STATES_NUM) << 1.0f, SAMPLING_PERIOD,
+ 0.5f * SAMPLING_PERIOD * SAMPLING_PERIOD, 0.0f, 1.0f, SAMPLING_PERIOD,
+ 0.0f, 0.0f, 1.0f)
+ .finished();
// Output matrix
-static const MatrixBase<float, 1, 3> C_INIT{1, 0, 0};
-static const MatrixBase<float, 2, 3> C_INIT_ACC{1, 0, 0, 0, 0, 1};
+static const MatrixPN H_INIT{1, 0, 0};
// Initial error covariance matrix
-static const MatrixBase<float, 3, 3> P_INIT{0.1, 0, 0, 0, 0, 0, 0, 0, 0};
-static const MatrixBase<float, 3, 3> P_INIT_ACC{0.1, 0, 0, 0, 0, 0, 0, 0, 100};
+static const MatrixNN P_INIT =
+ (MatrixNN(KALMAN_STATES_NUM, KALMAN_STATES_NUM) << 0.1, 0, 0, 0, 0, 0, 0, 0,
+ 0)
+ .finished();
// Model variance matrix
-static const MatrixBase<float, 3, 3> V1_INIT{1, 0, 0, 0, 10, 0, 0, 0, 100};
-static const MatrixBase<float, 3, 3> V1_INIT_ACC{0.1, 0, 0, 0, 80, 1, 0, 1, 10};
+static const MatrixNN Q_INIT =
+ (MatrixNN(KALMAN_STATES_NUM, KALMAN_STATES_NUM) << 1, 0, 0, 0, 10, 0, 0, 0,
+ 100)
+ .finished();
// Measurement variance
-static const MatrixBase<float, 1, 1> V2_INIT{800};
-static const MatrixBase<float, 2, 2> V2_INIT_ACC{1000,0,0,100};
+static const MatrixPP R_INIT{800};
+
+// method to initialize the kalman configuration structure
+static const KalmanEigen<float, KALMAN_STATES_NUM,
+ KALMAN_OUTPUTS_NUM>::KalmanConfig
+getKalmanConfig(const float ref_pressure)
+{
+ KalmanEigen<float, KALMAN_STATES_NUM, KALMAN_OUTPUTS_NUM>::KalmanConfig
+ config;
+ config.F = F_INIT;
+ config.H = H_INIT;
+ config.Q = Q_INIT;
+ config.R = R_INIT;
+ config.P = P_INIT;
+ config.x = CVectorN(ref_pressure, 0, KALMAN_INITIAL_ACCELERATION);
+
+ return config;
+}
+
+} // namespace ADAConfigs
-// Initialize the Kalman filter with a negative (pressure) acceleration in order
-// to make it more respondive during the propulsive phase
-static const float KALMAN_INITIAL_ACCELERATION = -500;
} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/configs/AeroBrakesConfig.h b/src/boards/DeathStack/configs/AeroBrakesConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..63c7317d0e23390f60103399f42a8992b85aa451
--- /dev/null
+++ b/src/boards/DeathStack/configs/AeroBrakesConfig.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Vincenzo Santomarco
+ *
+ * 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 "Constants.h"
+#include "drivers/HardwareTimer.h"
+#include "drivers/pwm/pwm.h"
+
+namespace DeathStackBoard
+{
+namespace AeroBrakesConfigs
+{
+
+static const PWM::Timer SERVO_TIMER{
+ TIM8, &(RCC->APB2ENR), RCC_APB2ENR_TIM8EN,
+ TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)};
+
+static constexpr PWMChannel SERVO_PWM_CH = PWMChannel::CH2;
+
+static constexpr uint32_t LOOKS = 50;
+static constexpr uint32_t START_INDEX_OFFSET = -1;
+static constexpr float M = 22; /**< rocket's mass */
+static constexpr float D = 0.15; /**< rocket's diameter */
+static constexpr float S0 = (PI * D * D) / 4;
+static constexpr float RHO = 1.225;
+static constexpr float Hn = 10400;
+static constexpr float Kp = 77;
+static constexpr float Ki = 5;
+static constexpr float Co = 340.3;
+static constexpr float ALPHA = -3.871e-3;
+static constexpr float A = -1.04034;
+static constexpr float B = 0.30548;
+static constexpr float A_DELTAS = -9.43386 / 1000;
+static constexpr float B_DELTAS = 19.86779 / 1000;
+static constexpr float DELTA_S_AVAILABLE_MIN = 0;
+static constexpr float DELTA_S_AVAILABLE_MAX = 0.01;
+static constexpr float DELTA_S_AVAILABLE_STEP = 0.0005;
+
+static constexpr float SERVO_MAX_POS = 50.99; // deg, 0.89 in radians
+static constexpr float SERVO_MIN_POS = 0; // deg
+static constexpr float SERVO_MAX_RATE = 60 / 0.2; // deg/s
+static constexpr float SERVO_MIN_RATE = -60 / 0.2; // deg/s
+static constexpr float SERVO_WIGGLE_AMPLITUDE = 10; // deg, 0.17 in radians
+
+static constexpr float UPDATE_TIME = 0.05 * 1000; // ms -> 20 Hz
+
+static constexpr int SHADOW_MODE_DURATION = 7500;
+
+} // namespace AeroBrakesConfigs
+
+} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/configs/CutterConfig.h b/src/boards/DeathStack/configs/CutterConfig.h
index 2f53d859a2b3b5ba623ae7a1a0ff504c070dd285..113a346404f21d92e1ff45e1687fa960aef181aa 100644
--- a/src/boards/DeathStack/configs/CutterConfig.h
+++ b/src/boards/DeathStack/configs/CutterConfig.h
@@ -29,39 +29,32 @@
namespace DeathStackBoard
{
-// clang-format off
-// Struct required by the PWM driver to know the specifics of the timer to use
-static const PWM::Timer CUTTER_TIM{
- TIM9,
- &(RCC->APB2ENR),
- RCC_APB2ENR_TIM9EN,
- TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)
- };
-
-// clang-format on
+namespace CutterConfig
+{
-static constexpr int CUT_DURATION = 5 * 1000;
-static constexpr int CUT_TEST_DURATION = 3 * 1000;
+static const PWM::Timer CUTTER_TIM{
+ TIM9, &(RCC->APB2ENR), RCC_APB2ENR_TIM9EN,
+ TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)};
// PRIMARY --> THCUT1 on theboard
-static const PWMChannel CUTTER_CHANNEL_PRIMARY = PWMChannel::CH2;
-typedef miosix::actuators::thCut1::ena PrimaryCutterEna;
+static const PWMChannel CUTTER_CHANNEL_PRIMARY = PWMChannel::CH2;
+typedef miosix::actuators::nosecone::thCut1::ena PrimaryCutterEna;
// BACKUP --> THCUT2 on theboard
static const PWMChannel CUTTER_CHANNEL_BACKUP = PWMChannel::CH2;
-typedef miosix::actuators::thCut2::ena BackupCutterEna;
+typedef miosix::actuators::nosecone::thCut2::ena BackupCutterEna;
-// PWM Frequency & duty-cycle
-static const unsigned int CUTTER_PWM_FREQUENCY = 15000; // Hz
-// Duty cycle to be used during flight to cut the chord
-static constexpr float CUTTER_PWM_DUTY_CYCLE = 0.31f;
+static constexpr int CUT_DURATION = 5 * 1000;
+static constexpr int CUT_TEST_DURATION = 3 * 1000;
-// Duty cycle to be used during integration, to perform a a non-destructive
-// continuity check
-static constexpr float CUTTER_TEST_PWM_DUTY_CYCLE = 0.05f;
+static const unsigned int PRIMARY_CUTTER_PWM_FREQUENCY = 15000; // Hz
+static constexpr float PRIMARY_CUTTER_PWM_DUTY_CYCLE = 0.31f;
+static const unsigned int BACKUP_CUTTER_PWM_FREQUENCY = 15000; // Hz
+static constexpr float BACKUP_CUTTER_PWM_DUTY_CYCLE = 0.31f;
+static constexpr float CUTTER_TEST_PWM_DUTY_CYCLE =
+ PRIMARY_CUTTER_PWM_DUTY_CYCLE / 100.0f;
-// Period of time where the IN must be kept low before bringing ENA/INH low
-static const int CUTTER_DISABLE_DELAY_MS = 50;
+} // namespace CutterConfig
-} // namespace DeathStackBoard
+} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/configs/DeploymentConfig.h b/src/boards/DeathStack/configs/DeploymentConfig.h
index 0f925deec3b16b7b10713bb95293f5b3f366aae7..de3c3db10cd8fc67166754f3b25cc593f28b6f25 100644
--- a/src/boards/DeathStack/configs/DeploymentConfig.h
+++ b/src/boards/DeathStack/configs/DeploymentConfig.h
@@ -1,6 +1,6 @@
/*
* Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+ * Authors: Luca Erbetta, Alberto Nidasio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -25,10 +25,6 @@
#include <drivers/HardwareTimer.h>
#include <drivers/pwm/pwm.h>
-#include <interfaces-impl/hwmapping.h>
-#include <miosix.h>
-#include "DeploymentController/Motor/MotorData.h"
-#include "config.h"
namespace DeathStackBoard
{
@@ -36,22 +32,22 @@ namespace DeathStackBoard
namespace DeploymentConfigs
{
-static constexpr uint8_t MAX_EJECTION_ATTEMPTS = 1;
-
-static constexpr int NC_OPEN_TIMEOUT = 5000;
-
static const PWM::Timer SERVO_TIMER{
TIM4, &(RCC->APB1ENR), RCC_APB1ENR_TIM4EN,
TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)};
-static constexpr PWMChannel SERVO_CHANNEL = PWMChannel::CH1;
+static constexpr PWMChannel SERVO_PWM_CH = PWMChannel::CH1;
+
+static constexpr int NC_OPEN_TIMEOUT = 5000;
-// Servo rest position
-static constexpr float SERVO_RESET_POS = 0.77f;
-static constexpr float SERVO_WIGGLE_AMPLITUDE = 0.02f;
+// Angles in degrees
+static constexpr float SERVO_MIN_POS = 63;
+static constexpr float SERVO_MAX_POS = 138.6;
+static constexpr float SERVO_RESET_POS = 138.6;
+static constexpr float SERVO_EJECT_POS = 63;
+static constexpr float SERVO_WIGGLE_AMPLITUDE = 5;
-// Servo position when ejecting the nosecone
-static constexpr float SERVO_EJECT_POS = 0.35f;
+static constexpr float UPDATE_TIME = 0.1 * 1000; // ms
} // namespace DeploymentConfigs
diff --git a/src/boards/DeathStack/configs/FlightStatsConfig.h b/src/boards/DeathStack/configs/FlightStatsConfig.h
index 06746ce4dbc29ca5960fbf735c29ccfb6c9c6091..05dde67ae6a9f154dda0a64c6f661590b60c27f8 100644
--- a/src/boards/DeathStack/configs/FlightStatsConfig.h
+++ b/src/boards/DeathStack/configs/FlightStatsConfig.h
@@ -23,19 +23,20 @@
#pragma once
-#include "config.h"
#include "CutterConfig.h"
+#include "config.h"
namespace DeathStackBoard
{
namespace FlightStatsConfig
{
-static constexpr long long TIMEOUT_CUTTER_TEST_STATS = CUT_TEST_DURATION;
-static constexpr long long TIMEOUT_LIFTOFF_STATS = 6000;
-static constexpr long long TIMEOUT_APOGEE_STATS = 1500;
-static constexpr long long TIMEOUT_DROGUE_DPL_STATS = 15000;
-static constexpr long long TIMEOUT_MAIN_DPL_STATS = 15000;
+static constexpr long long TIMEOUT_CUTTER_TEST_STATS =
+ 5000; // CUT_TEST_DURATION;
+static constexpr long long TIMEOUT_LIFTOFF_STATS = 6000;
+static constexpr long long TIMEOUT_APOGEE_STATS = 1500;
+static constexpr long long TIMEOUT_DROGUE_DPL_STATS = 15000;
+static constexpr long long TIMEOUT_MAIN_DPL_STATS = 15000;
} // namespace FlightStatsConfig
} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/configs/PinObserverConfig.h b/src/boards/DeathStack/configs/PinObserverConfig.h
index 3a613bcef5ced2696cd308b8b778930ce17a034c..126a005d413edc919a484151208a1038942b7a6f 100644
--- a/src/boards/DeathStack/configs/PinObserverConfig.h
+++ b/src/boards/DeathStack/configs/PinObserverConfig.h
@@ -1,6 +1,6 @@
/*
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+ * Copyright (c) 2019-2021 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -33,23 +33,27 @@ namespace DeathStackBoard
static const unsigned int PIN_POLL_INTERVAL = 10; // ms
// Launch pin config
-static const unsigned int PORT_LAUNCH_PIN = GPIOC_BASE;
-static const unsigned char NUM_LAUNCH_PIN = 6;
-
+static const GpioPin launch_pin(miosix::inputs::lp_dtch::getPin());
static const PinObserver::Transition TRIGGER_LAUNCH_PIN =
PinObserver::Transition::FALLING_EDGE;
-
// How many consecutive times the launch pin should be detected as detached
// before triggering a launch event.
static const unsigned int THRESHOLD_LAUNCH_PIN = 10;
-static const unsigned int PORT_NC_DETACH_PIN = GPIOB_BASE;
-static const unsigned char NUM_NC_DETACH_PIN = 7;
+// Nosecone detach pin config
+static const GpioPin nosecone_pin(miosix::inputs::nc_dtch::getPin());
static const PinObserver::Transition TRIGGER_NC_DETACH_PIN =
PinObserver::Transition::FALLING_EDGE;
-
// How many consecutive times the nosecone pin should be detected as detached
// before triggering a nosecone detach event.
static const unsigned int THRESHOLD_NC_DETACH_PIN = 10;
+// Dpl servo actuation pin config
+static const GpioPin dpl_servo_pin(miosix::inputs::expulsion_in::getPin());
+static const PinObserver::Transition TRIGGER_DPL_SERVO_PIN =
+ PinObserver::Transition::FALLING_EDGE;
+// How many consecutive times the deployment servo pin should be detected as
+// detached before triggering a nosecone detach event.
+static const unsigned int THRESHOLD_DPL_SERVO_PIN = 10;
+
} // namespace DeathStackBoard
\ No newline at end of file
diff --git a/src/boards/DeathStack/configs/SensorManagerConfig.h b/src/boards/DeathStack/configs/SensorManagerConfig.h
index f51381307293a1a259a694b643e620d289716a3e..6e368d3f962d3dba1868f6677214b7a9c0ad44bb 100644
--- a/src/boards/DeathStack/configs/SensorManagerConfig.h
+++ b/src/boards/DeathStack/configs/SensorManagerConfig.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015-2018 Skyward Experimental Rocketry
+/* Copyright (c) 2015-2021 Skyward Experimental Rocketry
* Authors: Luca Erbetta
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -23,65 +23,14 @@
#pragma once
#include <Common.h>
-#include <drivers/BusTemplate.h>
-#include <drivers/stm32f2_f4_i2c.h>
#include <interfaces-impl/hwmapping.h>
-#include <drivers/SoftwareI2CAdapter.h>
using miosix::Gpio;
namespace DeathStackBoard
{
-// I2C 1
-typedef SoftwareI2CAdapter<miosix::interfaces::i2c::sda, miosix::interfaces::i2c::scl> i2c1;
-// SPI1
-typedef BusSPI<1, miosix::interfaces::spi1::mosi,
- miosix::interfaces::spi1::miso, miosix::interfaces::spi1::sck>
- busSPI1;
-
-// Spi protocol defs
-typedef ProtocolSPI<busSPI1, miosix::sensors::mpu9250::cs> spiMPU9250;
-typedef ProtocolSPI<busSPI1, miosix::sensors::adis16405::cs> spiADIS16405;
-typedef ProtocolSPI<busSPI1, miosix::sensors::ms5803::cs> spiMS5803;
-
-typedef miosix::sensors::ad7994::ab ad7994_busy_pin;
-typedef miosix::sensors::ad7994::nconvst ad7994_nconvst;
-
-static constexpr uint8_t AD7994_NXP_BARO_CHANNEL = 1;
-static constexpr uint8_t AD7994_HONEYWELL_BARO_CHANNEL = 3;
-
-
-static constexpr uint8_t INTERNAL_ADC_NUM = 3;
-static constexpr uint8_t ADC_CURRENT_SENSE_1_CHANNEL = 6;
-static constexpr uint8_t ADC_CURRENT_SENSE_2_CHANNEL = 4;
-static constexpr uint8_t ADC_BATTERY_VOLTAGE_CHANNEL = 5;
-
-//Time after which a gps location is no longer valid in milliseconds
-static constexpr long long MAX_GPS_FIX_AGE = 300;
-
-#ifdef DEATH_STACK_2
-static constexpr float AD7994_V_REF = 4.29f;
-
-
-static constexpr float OFFSET_MPU_ACC_X = -0.37f;
-static constexpr float OFFSET_MPU_ACC_Y = 0.29f;
-static constexpr float OFFSET_MPU_ACC_Z = 2.92f;
-
-#else
-static constexpr float AD7994_V_REF = 4.21f;
-
-static constexpr float OFFSET_MPU_ACC_X = 0.0f;
-static constexpr float OFFSET_MPU_ACC_Y = 0.0f;
-static constexpr float OFFSET_MPU_ACC_Z = 0.0f;
-
-#ifndef DEATH_STACK_1
-#ifdef _BOARD_STM32F429ZI_SKYWARD_DEATHST
-#warning "You have not specified which stack you are using! (-DDEATH_STACK_1 OR -DDEATH_STACK_2)"
-#endif
-#endif
-#endif
} // namespace DeathStackBoard
diff --git a/src/boards/DeathStack/configs/TMTCConfig.h b/src/boards/DeathStack/configs/TMTCConfig.h
index a3690c0c838e8d9c1022f66fa6989e90ce81e4c2..f274fdcf1016a1c0c250ed91f509cf8687569f2a 100644
--- a/src/boards/DeathStack/configs/TMTCConfig.h
+++ b/src/boards/DeathStack/configs/TMTCConfig.h
@@ -21,29 +21,26 @@
*/
#pragma once
-#include <drivers/BusTemplate.h>
#include <interfaces-impl/hwmapping.h>
#include <skyward-boardcore/libs/mavlink_skyward_lib/mavlink_lib/hermes/mavlink.h>
+
#include "drivers/Xbee/Xbee.h"
+#include "drivers/spi/SPIDriver.h"
namespace DeathStackBoard
{
/* Mavlink Driver queue settings */
-static constexpr unsigned int MAV_OUT_QUEUE_LEN = 10;
-static constexpr unsigned int MAV_PKT_SIZE = 255;
-static constexpr long long MAV_OUT_BUFFER_MAX_AGE = 200;
+static constexpr unsigned int MAV_OUT_QUEUE_LEN = 10;
+static constexpr unsigned int MAV_PKT_SIZE = 255;
+static constexpr long long MAV_OUT_BUFFER_MAX_AGE = 200;
/* Min guaranteed sleep time after each packet sent(milliseconds) */
static const uint16_t TMTC_SLEEP_AFTER_SEND = 0;
-/* Device */
-typedef BusSPI<2, miosix::interfaces::spi2::mosi,
- miosix::interfaces::spi2::miso, miosix::interfaces::spi2::sck>
- busSPI2;
-
-typedef Xbee::Xbee<busSPI2, miosix::xbee::cs, miosix::xbee::attn,
- miosix::xbee::reset>
- Xbee_t;
+/* Xbee */
+typedef miosix::xbee::cs XbeeCS;
+typedef miosix::xbee::attn XbeeATTN;
+typedef miosix::xbee::reset XbeeRST;
/* Periodic telemetries periods */
static const unsigned int LR_TM_TIMEOUT = 1000;
diff --git a/src/boards/DeathStack/events/EventInjector.h b/src/boards/DeathStack/events/EventInjector.h
index 98cd454391e843a1d8a3ad266ce1c4b947910bc7..5aba27b0fac509c59e44c2b0d9920fd32797e89b 100644
--- a/src/boards/DeathStack/events/EventInjector.h
+++ b/src/boards/DeathStack/events/EventInjector.h
@@ -1,5 +1,5 @@
/**
- * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Copyright (c) 2019-2021 Skyward Experimental Rocketry
* Authors: Luca Erbetta
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
diff --git a/src/boards/DeathStack/events/EventStrings.cpp b/src/boards/DeathStack/events/EventStrings.cpp
index 495e1ead073c1631b3cb3f74c7ab4cc955c57694..1cc170e750937d4aa1f1103bcd030060edecde80 100644
--- a/src/boards/DeathStack/events/EventStrings.cpp
+++ b/src/boards/DeathStack/events/EventStrings.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+/* Copyright (c) 2018-2020 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, 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
@@ -26,104 +26,71 @@
******************************************************************************
*/
-// Generated from: https://docs.google.com/spreadsheets/d/184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0
-// Autogen date: 2019-11-06 23:09:09.820014
-
-
-#include "Events.h"
-#include "Topics.h"
+// Autogen date: 2021-04-17 23:09:11.522877
#include <map>
using std::map;
-namespace DeathStackBoard
-{
+#include "Events.h"
+#include "Topics.h"
string getEventString(uint8_t event)
{
- static const map<uint8_t, string> event_string_map {
- { EV_ADA_APOGEE_DETECTED, "EV_ADA_APOGEE_DETECTED" },
- { EV_ADA_DPL_ALT_DETECTED, "EV_ADA_DPL_ALT_DETECTED" },
- { EV_ADA_READY, "EV_ADA_READY" },
- { EV_APOGEE, "EV_APOGEE" },
- { EV_ARMED, "EV_ARMED" },
- { EV_BUTTON_DOWN, "EV_BUTTON_DOWN" },
- { EV_BUTTON_LONG_PRESS, "EV_BUTTON_LONG_PRESS" },
- { EV_BUTTON_SHORT_PRESS, "EV_BUTTON_SHORT_PRESS" },
- { EV_BUTTON_UP, "EV_BUTTON_UP" },
- { EV_BUTTON_VERY_LONG_PRESS, "EV_BUTTON_VERY_LONG_PRESS" },
- { EV_CALIBRATE_ADA, "EV_CALIBRATE_ADA" },
- { EV_CUT_BACKUP, "EV_CUT_BACKUP" },
- { EV_CUT_DROGUE, "EV_CUT_DROGUE" },
- { EV_CUT_PRIMARY, "EV_CUT_PRIMARY" },
- { EV_DISARMED, "EV_DISARMED" },
- { EV_DPL_ALTITUDE, "EV_DPL_ALTITUDE" },
- { EV_FLIGHTSTATS_TIMEOUT, "EV_FLIGHTSTATS_TIMEOUT" },
- { EV_INIT_ERROR, "EV_INIT_ERROR" },
- { EV_INIT_OK, "EV_INIT_OK" },
- { EV_LANDED, "EV_LANDED" },
- { EV_LIFTOFF, "EV_LIFTOFF" },
- { EV_NC_DETACHED, "EV_NC_DETACHED" },
- { EV_NC_OPEN, "EV_NC_OPEN" },
- { EV_RESET_SERVO, "EV_RESET_SERVO" },
- { EV_SEND_HR_TM, "EV_SEND_HR_TM" },
- { EV_SEND_LR_TM, "EV_SEND_LR_TM" },
- { EV_SEND_TEST_TM, "EV_SEND_TEST_TM" },
- { EV_TC_ARM, "EV_TC_ARM" },
- { EV_TC_BOARD_RESET, "EV_TC_BOARD_RESET" },
- { EV_TC_CALIBRATE_ADA, "EV_TC_CALIBRATE_ADA" },
- { EV_TC_CLOSE_LOG, "EV_TC_CLOSE_LOG" },
- { EV_TC_CUT_BACKUP, "EV_TC_CUT_BACKUP" },
- { EV_TC_CUT_DROGUE, "EV_TC_CUT_DROGUE" },
- { EV_TC_CUT_PRIMARY, "EV_TC_CUT_PRIMARY" },
- { EV_TC_DISARM, "EV_TC_DISARM" },
- { EV_TC_END_MISSION, "EV_TC_END_MISSION" },
- { EV_TC_FORCE_INIT, "EV_TC_FORCE_INIT" },
- { EV_TC_LAUNCH, "EV_TC_LAUNCH" },
- { EV_TC_NC_CLOSE, "EV_TC_NC_CLOSE" },
- { EV_TC_NC_OPEN, "EV_TC_NC_OPEN" },
- { EV_TC_RESET_SERVO, "EV_TC_RESET_SERVO" },
- { EV_TC_SET_DPL_ALTITUDE, "EV_TC_SET_DPL_ALTITUDE" },
- { EV_TC_SET_REFERENCE_ALTITUDE, "EV_TC_SET_REFERENCE_ALTITUDE" },
- { EV_TC_SET_REFERENCE_TEMP, "EV_TC_SET_REFERENCE_TEMP" },
- { EV_TC_START_ROGALLO_CONTROL, "EV_TC_START_ROGALLO_CONTROL" },
- { EV_TC_START_SENSOR_LOGGING, "EV_TC_START_SENSOR_LOGGING" },
- { EV_TC_STOP_SENSOR_LOGGING, "EV_TC_STOP_SENSOR_LOGGING" },
- { EV_TC_TEST_CUTTER_BACKUP, "EV_TC_TEST_CUTTER_BACKUP" },
- { EV_TC_TEST_CUTTER_PRIMARY, "EV_TC_TEST_CUTTER_PRIMARY" },
- { EV_TC_TEST_MODE, "EV_TC_TEST_MODE" },
- { EV_TC_WIGGLE_SERVO, "EV_TC_WIGGLE_SERVO" },
- { EV_TEST_CUTTER_BACKUP, "EV_TEST_CUTTER_BACKUP" },
- { EV_TEST_CUTTER_PRIMARY, "EV_TEST_CUTTER_PRIMARY" },
- { EV_TEST_MODE, "EV_TEST_MODE" },
- { EV_TIMEOUT_CUTTING, "EV_TIMEOUT_CUTTING" },
- { EV_TIMEOUT_END_MISSION, "EV_TIMEOUT_END_MISSION" },
- { EV_TIMEOUT_NC_OPEN, "EV_TIMEOUT_NC_OPEN" },
- { EV_TIMEOUT_PRESS_STABILIZATION, "EV_TIMEOUT_PRESS_STABILIZATION" },
- { EV_TIMEOUT_SERVO_RESET, "EV_TIMEOUT_SERVO_RESET" },
- { EV_TIMEOUT_SHADOW_MODE, "EV_TIMEOUT_SHADOW_MODE" },
- { EV_UMBILICAL_DETACHED, "EV_UMBILICAL_DETACHED" },
- { EV_WIGGLE_SERVO, "EV_WIGGLE_SERVO" }
+ static const map<uint8_t, string> event_string_map{
+ {EV_CALIBRATE, "EV_CALIBRATE"},
+ {EV_CALIBRATE_SENSORS, "EV_CALIBRATE_SENSORS"},
+ {EV_ARMED, "EV_ARMED"},
+ {EV_TC_START_SENSOR_LOGGING, "EV_TC_START_SENSOR_LOGGING"},
+ {EV_SM_READY, "EV_SM_READY"},
+ {EV_LANDED, "EV_LANDED"},
+ {EV_TC_STOP_SENSOR_LOGGING, "EV_TC_STOP_SENSOR_LOGGING"},
+ {EV_RESET_SERVO, "EV_RESET_SERVO"},
+ {EV_WIGGLE_SERVO, "EV_WIGGLE_SERVO"},
+ {EV_NC_OPEN, "EV_NC_OPEN"},
+ {EV_CUT_DROGUE, "EV_CUT_DROGUE"},
+ {EV_TEST_CUT_PRIMARY, "EV_TEST_CUT_PRIMARY"},
+ {EV_TEST_CUT_BACKUP, "EV_TEST_CUT_BACKUP"},
+ {EV_NC_DETACHED, "EV_NC_DETACHED"},
+ {EV_NC_OPEN_TIMEOUT, "EV_NC_OPEN_TIMEOUT"},
+ {EV_CUTTING_TIMEOUT, "EV_CUTTING_TIMEOUT"},
+ {EV_LIFTOFF, "EV_LIFTOFF"},
+ {EV_TEST_ABK, "EV_TEST_ABK"},
+ {EV_SHADOW_MODE_TIMEOUT, "EV_SHADOW_MODE_TIMEOUT"},
+ {EV_DISABLE_ABK, "EV_DISABLE_ABK"},
+ {EV_APOGEE, "EV_APOGEE"},
+ {EV_TEST_TIMEOUT, "EV_TEST_TIMEOUT"},
+ {EV_CALIBRATE_NAS, "EV_CALIBRATE_NAS"},
+ {EV_NAS_READY, "EV_NAS_READY"},
+ {EV_SEND_TEST_TM, "EV_SEND_TEST_TM"},
+ {EV_SEND_HR_TM, "EV_SEND_HR_TM"},
+ {EV_SEND_LR_TM, "EV_SEND_LR_TM"},
+ {EV_DISARMED, "EV_DISARMED"},
+ {EV_DPL_ALTITUDE, "EV_DPL_ALTITUDE"},
+ {EV_STATS_TIMEOUT, "EV_STATS_TIMEOUT"},
+ {EV_CALIBRATE_ADA, "EV_CALIBRATE_ADA"},
+ {EV_ADA_READY, "EV_ADA_READY"},
+ {EV_TIMEOUT_SHADOW_MODE, "EV_TIMEOUT_SHADOW_MODE"},
+ {EV_ADA_APOGEE_DETECTED, "EV_ADA_APOGEE_DETECTED"},
+ {EV_TIMEOUT_PRESS_STABILIZATION, "EV_TIMEOUT_PRESS_STABILIZATION"},
+ {EV_ADA_DPL_ALT_DETECTED, "EV_ADA_DPL_ALT_DETECTED"},
};
- auto it = event_string_map.find(event);
+ auto it = event_string_map.find(event);
return it == event_string_map.end() ? "EV_UNKNOWN" : it->second;
}
string getTopicString(uint8_t topic)
{
static const map<uint8_t, string> topic_string_map{
- { TOPIC_ADA, "TOPIC_ADA" },
- { TOPIC_CAN, "TOPIC_CAN" },
- { TOPIC_DEPLOYMENT, "TOPIC_DEPLOYMENT" },
- { TOPIC_FLIGHT_EVENTS, "TOPIC_FLIGHT_EVENTS" },
- { TOPIC_FMM, "TOPIC_FMM" },
- { TOPIC_IGNITION, "TOPIC_IGNITION" },
- { TOPIC_STATS, "TOPIC_STATS" },
- { TOPIC_TC, "TOPIC_TC" },
- { TOPIC_TMTC, "TOPIC_TMTC" }
+ {TOPIC_FLIGHT_EVENTS, "TOPIC_FLIGHT_EVENTS"},
+ {TOPIC_SM, "TOPIC_SM"},
+ {TOPIC_TC, "TOPIC_TC"},
+ {TOPIC_DPL, "TOPIC_DPL"},
+ {TOPIC_ABK, "TOPIC_ABK"},
+ {TOPIC_NAS, "TOPIC_NAS"},
+ {TOPIC_TMTC, "TOPIC_TMTC"},
+ {TOPIC_STATS, "TOPIC_STATS"},
+ {TOPIC_ADA, "TOPIC_ADA"},
};
auto it = topic_string_map.find(topic);
- return it == topic_string_map.end() ? "TOPIC_UNKNOWN" : it->second;
-}
-
+ return it == topic_string_map.end() ? "TOPIC_UNKNOWN" : it->second;
}
\ No newline at end of file
diff --git a/src/boards/DeathStack/events/Events.h b/src/boards/DeathStack/events/Events.h
index 644c10376b3507c5b1df7faee1be93d37bb8dcc2..1c8507d7fd44230d9fed565979c5f9dce6646e0b 100644
--- a/src/boards/DeathStack/events/Events.h
+++ b/src/boards/DeathStack/events/Events.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+/* Copyright (c) 2018-2021 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, 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
@@ -26,8 +26,7 @@
******************************************************************************
*/
-// Generated from: https://docs.google.com/spreadsheets/d/184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0
-// Autogen date: 2019-11-06 23:09:09.820014
+// Autogen date: 2021-04-17 23:09:11.522877
#pragma once
@@ -35,47 +34,52 @@
#include <string>
#include <vector>
+#include "Topics.h"
#include "events/Event.h"
#include "events/EventBroker.h"
-#include "Topics.h"
using std::string;
-namespace DeathStackBoard
-{
/**
* Definition of all events in the Homeone Board software
* Refer to section 5.1.1 of the Software Design Document.
*/
enum Events : uint8_t
{
- EV_ADA_APOGEE_DETECTED = EV_FIRST_SIGNAL,
- EV_ADA_DPL_ALT_DETECTED,
- EV_ADA_READY,
- EV_APOGEE,
+ EV_CALIBRATE = EV_FIRST_SIGNAL,
+ EV_CALIBRATE_SENSORS,
EV_ARMED,
- EV_BUTTON_DOWN,
- EV_BUTTON_LONG_PRESS,
- EV_BUTTON_SHORT_PRESS,
- EV_BUTTON_UP,
- EV_BUTTON_VERY_LONG_PRESS,
- EV_CALIBRATE_ADA,
- EV_CUT_BACKUP,
+ EV_TC_START_SENSOR_LOGGING,
+ EV_SM_READY,
+ EV_LANDED,
+ EV_TC_STOP_SENSOR_LOGGING,
+ EV_RESET_SERVO,
+ EV_WIGGLE_SERVO,
+ EV_NC_OPEN,
EV_CUT_DROGUE,
- EV_CUT_PRIMARY,
+ EV_TEST_CUT_PRIMARY,
+ EV_TEST_CUT_BACKUP,
+ EV_NC_DETACHED,
+ EV_NC_OPEN_TIMEOUT,
+ EV_CUTTING_TIMEOUT,
+ EV_LIFTOFF,
+ EV_TEST_ABK,
+ EV_SHADOW_MODE_TIMEOUT,
+ EV_DISABLE_ABK,
+ EV_APOGEE,
+ EV_TEST_TIMEOUT,
+ EV_CALIBRATE_NAS,
+ EV_NAS_READY,
+ EV_SEND_TEST_TM,
+ EV_SEND_HR_TM,
+ EV_SEND_LR_TM,
EV_DISARMED,
EV_DPL_ALTITUDE,
- EV_FLIGHTSTATS_TIMEOUT,
+ EV_STATS_TIMEOUT,
EV_INIT_ERROR,
EV_INIT_OK,
- EV_LANDED,
- EV_LIFTOFF,
- EV_NC_DETACHED,
- EV_NC_OPEN,
- EV_RESET_SERVO,
- EV_SEND_HR_TM,
- EV_SEND_LR_TM,
- EV_SEND_TEST_TM,
+ EV_CUT_BACKUP,
+ EV_CUT_PRIMARY,
EV_TC_ARM,
EV_TC_BOARD_RESET,
EV_TC_CALIBRATE_ADA,
@@ -87,15 +91,11 @@ enum Events : uint8_t
EV_TC_END_MISSION,
EV_TC_FORCE_INIT,
EV_TC_LAUNCH,
- EV_TC_NC_CLOSE,
EV_TC_NC_OPEN,
EV_TC_RESET_SERVO,
EV_TC_SET_DPL_ALTITUDE,
EV_TC_SET_REFERENCE_ALTITUDE,
EV_TC_SET_REFERENCE_TEMP,
- EV_TC_START_ROGALLO_CONTROL,
- EV_TC_START_SENSOR_LOGGING,
- EV_TC_STOP_SENSOR_LOGGING,
EV_TC_TEST_CUTTER_BACKUP,
EV_TC_TEST_CUTTER_PRIMARY,
EV_TC_TEST_MODE,
@@ -103,24 +103,59 @@ enum Events : uint8_t
EV_TEST_CUTTER_BACKUP,
EV_TEST_CUTTER_PRIMARY,
EV_TEST_MODE,
- EV_TIMEOUT_CUTTING,
EV_TIMEOUT_END_MISSION,
- EV_TIMEOUT_NC_OPEN,
- EV_TIMEOUT_PRESS_STABILIZATION,
- EV_TIMEOUT_SERVO_RESET,
- EV_TIMEOUT_SHADOW_MODE,
EV_UMBILICAL_DETACHED,
- EV_WIGGLE_SERVO
+ EV_CALIBRATE_ADA,
+ EV_ADA_READY,
+ EV_TIMEOUT_SHADOW_MODE,
+ EV_ADA_APOGEE_DETECTED,
+ EV_TIMEOUT_PRESS_STABILIZATION,
+ EV_ADA_DPL_ALT_DETECTED,
};
-const std::vector<uint8_t> EVENT_LIST {EV_ADA_APOGEE_DETECTED, EV_ADA_DPL_ALT_DETECTED, EV_ADA_READY, EV_APOGEE, EV_ARMED, EV_BUTTON_DOWN, EV_BUTTON_LONG_PRESS, EV_BUTTON_SHORT_PRESS, EV_BUTTON_UP, EV_BUTTON_VERY_LONG_PRESS, EV_CALIBRATE_ADA, EV_CUT_BACKUP, EV_CUT_DROGUE, EV_CUT_PRIMARY, EV_DISARMED, EV_DPL_ALTITUDE, EV_FLIGHTSTATS_TIMEOUT, EV_INIT_ERROR, EV_INIT_OK, EV_LANDED, EV_LIFTOFF, EV_NC_DETACHED, EV_NC_OPEN, EV_RESET_SERVO, EV_SEND_HR_TM, EV_SEND_LR_TM, EV_SEND_TEST_TM, EV_TC_ARM, EV_TC_BOARD_RESET, EV_TC_CALIBRATE_ADA, EV_TC_CLOSE_LOG, EV_TC_CUT_BACKUP, EV_TC_CUT_DROGUE, EV_TC_CUT_PRIMARY, EV_TC_DISARM, EV_TC_END_MISSION, EV_TC_FORCE_INIT, EV_TC_LAUNCH, EV_TC_NC_CLOSE, EV_TC_NC_OPEN, EV_TC_RESET_SERVO, EV_TC_SET_DPL_ALTITUDE, EV_TC_SET_REFERENCE_ALTITUDE, EV_TC_SET_REFERENCE_TEMP, EV_TC_START_ROGALLO_CONTROL, EV_TC_START_SENSOR_LOGGING, EV_TC_STOP_SENSOR_LOGGING, EV_TC_TEST_CUTTER_BACKUP, EV_TC_TEST_CUTTER_PRIMARY, EV_TC_TEST_MODE, EV_TC_WIGGLE_SERVO, EV_TEST_CUTTER_BACKUP, EV_TEST_CUTTER_PRIMARY, EV_TEST_MODE, EV_TIMEOUT_CUTTING, EV_TIMEOUT_END_MISSION, EV_TIMEOUT_NC_OPEN, EV_TIMEOUT_PRESS_STABILIZATION, EV_TIMEOUT_SERVO_RESET, EV_TIMEOUT_SHADOW_MODE, EV_UMBILICAL_DETACHED, EV_WIGGLE_SERVO};
+const std::vector<uint8_t> EVENT_LIST{
+ EV_CALIBRATE,
+ EV_CALIBRATE_SENSORS,
+ EV_ARMED,
+ EV_TC_START_SENSOR_LOGGING,
+ EV_SM_READY,
+ EV_LANDED,
+ EV_TC_STOP_SENSOR_LOGGING,
+ EV_RESET_SERVO,
+ EV_WIGGLE_SERVO,
+ EV_NC_OPEN,
+ EV_CUT_DROGUE,
+ EV_TEST_CUT_PRIMARY,
+ EV_TEST_CUT_BACKUP,
+ EV_NC_DETACHED,
+ EV_NC_OPEN_TIMEOUT,
+ EV_CUTTING_TIMEOUT,
+ EV_LIFTOFF,
+ EV_TEST_ABK,
+ EV_SHADOW_MODE_TIMEOUT,
+ EV_DISABLE_ABK,
+ EV_APOGEE,
+ EV_TEST_TIMEOUT,
+ EV_CALIBRATE_NAS,
+ EV_NAS_READY,
+ EV_SEND_TEST_TM,
+ EV_SEND_HR_TM,
+ EV_SEND_LR_TM,
+ EV_DISARMED,
+ EV_DPL_ALTITUDE,
+ EV_STATS_TIMEOUT,
+ EV_CALIBRATE_ADA,
+ EV_ADA_READY,
+ EV_TIMEOUT_SHADOW_MODE,
+ EV_ADA_APOGEE_DETECTED,
+ EV_TIMEOUT_PRESS_STABILIZATION,
+ EV_ADA_DPL_ALT_DETECTED,
+};
/**
* @brief Returns the name of the provided event
- *
- * @param event
- * @return string
+ *
+ * @param event
+ * @return string
*/
-string getEventString(uint8_t event);
-
-}
+string getEventString(uint8_t event);
\ No newline at end of file
diff --git a/src/boards/DeathStack/events/Topics.h b/src/boards/DeathStack/events/Topics.h
index 48f9f322c3db48c9752d4a8013e6183eef7d3770..1cb350c34ee53fefb6ba2546286898a197e25030 100644
--- a/src/boards/DeathStack/events/Topics.h
+++ b/src/boards/DeathStack/events/Topics.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+/* Copyright (c) 2018-2021 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta, 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
@@ -26,36 +26,45 @@
******************************************************************************
*/
-// Generated from: https://docs.google.com/spreadsheets/d/184kR2OAD7yWV0fYJdiGUDmHmy5_prY3nr-XgNA0Uge0
-// Autogen date: 2019-11-06 23:09:09.820014
+// Autogen date: 2021-04-17 23:09:11.522877
#pragma once
-#include <stdint.h>
+#include <cstdint>
#include <string>
#include <vector>
using std::string;
-namespace DeathStackBoard
-{
/**
* Definition of various event topics to use in the EventBroker
*/
enum Topics : uint8_t
{
- TOPIC_ADA,
- TOPIC_CAN,
- TOPIC_DEPLOYMENT,
TOPIC_FLIGHT_EVENTS,
TOPIC_FMM,
- TOPIC_IGNITION,
- TOPIC_STATS,
+ TOPIC_SM,
TOPIC_TC,
- TOPIC_TMTC
+ TOPIC_DPL,
+ TOPIC_ABK,
+ TOPIC_NAS,
+ TOPIC_TMTC,
+ TOPIC_STATS,
+ TOPIC_ADA,
};
-const std::vector<uint8_t> TOPIC_LIST {TOPIC_ADA, TOPIC_CAN, TOPIC_DEPLOYMENT, TOPIC_FLIGHT_EVENTS, TOPIC_FMM, TOPIC_IGNITION, TOPIC_STATS, TOPIC_TC, TOPIC_TMTC};
+const std::vector<uint8_t> TOPIC_LIST{
+ TOPIC_FLIGHT_EVENTS,
+ TOPIC_FMM,
+ TOPIC_SM,
+ TOPIC_TC,
+ TOPIC_DPL,
+ TOPIC_ABK,
+ TOPIC_NAS,
+ TOPIC_TMTC,
+ TOPIC_STATS,
+ TOPIC_ADA,
+};
/**
* @brief Returns the name of the provided event
@@ -63,7 +72,4 @@ const std::vector<uint8_t> TOPIC_LIST {TOPIC_ADA, TOPIC_CAN, TOPIC_DEPLOYMENT, T
* @param event
* @return string
*/
-string getTopicString(uint8_t topic);
-
-} // namespace DeathStackBoard
-
+string getTopicString(uint8_t topic);
\ No newline at end of file
diff --git a/src/entrypoints/death-stack-entry.cpp b/src/entrypoints/death-stack-entry.cpp
index 55631178c9028985c0b8d56bdd69630727cdff76..35e837b9ac9bbf469d47bfad4e92c7ae52c313a0 100644
--- a/src/entrypoints/death-stack-entry.cpp
+++ b/src/entrypoints/death-stack-entry.cpp
@@ -38,7 +38,7 @@ DeathStack* board;
StatsResult cpu_stat_res;
-D(EventInjector debug_console);
+D(EventInjector debug_console)
int main()
{
diff --git a/src/tests/aerobrakes/test-aerobrakes-algorithm.cpp b/src/tests/aerobrakes/test-aerobrakes-algorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..446c4454ed081285c46631768401342b4eb65861
--- /dev/null
+++ b/src/tests/aerobrakes/test-aerobrakes-algorithm.cpp
@@ -0,0 +1,157 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Vincenzo Santomarco
+ *
+ * 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 <miosix.h>
+
+#include <cmath>
+#include <cstdio>
+#include <iostream>
+#include <sstream>
+#include <string>
+
+#define private public
+
+#include "AeroBrakesController/AeroBrakesControlAlgorithm.h"
+#include "AeroBrakesController/AeroBrakesServo.h"
+#include "drivers/HardwareTimer.h"
+#include "sensors/Sensor.h"
+#include "test_data.h"
+
+using namespace DeathStackBoard;
+using namespace std;
+class MockActuator : public AeroBrakesServo
+{
+public:
+ MockActuator() {}
+ // void setPosition(float alpha) { lastAlpha = round(alpha * 180 / PI); }
+ float error(float alphaTest) { return abs(alphaTest - currentPosition); }
+
+ // private:
+ // int lastAlpha;
+};
+
+template <typename T>
+class MockSensor : public Sensor<T>
+{
+private:
+ int ts = 0;
+
+protected:
+ T sampleImpl() override
+ {
+ input_t input = DATA[ts].input;
+ T sampled;
+
+ sampled.z = input.z;
+ sampled.vz = input.vz;
+ sampled.vMod = input.vMod;
+ sampled.timestamp = ts;
+
+ ts++;
+
+ return sampled;
+ }
+
+ bool init() override { return true; }
+
+ bool selfTest() override { return true; }
+};
+
+class InputClass
+{
+public:
+ float z;
+ float vz;
+ float vMod;
+ uint64_t timestamp;
+};
+
+MockActuator mockActuator;
+MockSensor<InputClass> mockSensor;
+AeroBrakesControlAlgorithm<InputClass> algorithm(mockSensor, &mockActuator);
+
+int main()
+{
+ {
+ miosix::FastInterruptDisableLock dLock;
+
+ // Enable TIM5 peripheral clocks
+ RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
+ }
+
+ input_t input;
+
+ input = DATA[0].input;
+
+ HardwareTimer<uint32_t> hrclock(
+ TIM5,
+ TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1));
+ hrclock.setPrescaler(127);
+ hrclock.start();
+
+ mockSensor.sample();
+
+ uint32_t t1 = hrclock.tick();
+ algorithm.begin();
+ uint32_t t2 = hrclock.tick();
+
+ float startTime = hrclock.toMilliSeconds(t2 - t1);
+ float stepTime = 0;
+ int maxAlphaError = 0;
+
+ for (int i = 1; i < LEN_TEST; i++)
+ {
+ mockSensor.sample();
+ t1 = hrclock.tick();
+ algorithm.update();
+ t2 = hrclock.tick();
+
+ int error = mockActuator.error(DATA[i].output.alpha);
+
+ if (error > maxAlphaError)
+ {
+ maxAlphaError = error;
+ }
+
+ stepTime += hrclock.toMilliSeconds(t2 - t1);
+
+ cout << "z: " << mockSensor.getLastSample().z
+ << "\tvz: " << mockSensor.getLastSample().vz
+ << "\tvMod: " << mockSensor.getLastSample().vMod
+ << "\tservo: " << mockActuator.getCurrentPosition() << "\n";
+ }
+
+ miosix::Thread::sleep(AeroBrakesConfigs::UPDATE_TIME);
+
+ printf("Max alpha error: %d\n", maxAlphaError);
+
+ printf(
+ "Init time: %f ms\nAvg iter time: %f ms\nTotal computation time: %f "
+ "ms\n",
+ startTime, stepTime / (LEN_TEST - 1), stepTime + startTime);
+
+ // avoid reboot
+ while (1)
+ {
+ }
+}
\ No newline at end of file
diff --git a/src/tests/aerobrakes/test-aerobrakes-interactive.cpp b/src/tests/aerobrakes/test-aerobrakes-interactive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..50be6426fe5155fce0b03c73ad8ef91404c6d748
--- /dev/null
+++ b/src/tests/aerobrakes/test-aerobrakes-interactive.cpp
@@ -0,0 +1,372 @@
+/**
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio
+ *
+ * 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 <miosix.h>
+
+#include <cstdio>
+#include <iostream>
+#include <sstream>
+#include <string>
+
+#include "AeroBrakesController/AeroBrakesController.h"
+#include "AeroBrakesController/AeroBrakesServo.h"
+#include "events/Events.h"
+#include "test_data.h"
+#include "utils/testutils/TestHelper.h"
+
+using namespace DeathStackBoard;
+
+using namespace std;
+
+void testAlgorithm();
+void testAeroBrakes();
+void wiggleServo();
+void setServoFullyOpen();
+void setServoFullyClosed();
+void resetServo();
+void manualServoControl();
+void setServoParameters();
+void resetServoParameters();
+void waitUserInput();
+
+class InputClass
+{
+public:
+ float z;
+ float vz;
+ float vMod;
+ uint64_t timestamp;
+};
+
+template <typename T>
+class MockSensor : public Sensor<T>
+{
+private:
+ int ts = 0;
+
+protected:
+ T sampleImpl() override
+ {
+ input_t input = DATA[ts].input;
+ T sampled;
+
+ sampled.z = input.z;
+ sampled.vz = input.vz;
+ sampled.vMod = input.vMod;
+ sampled.timestamp = ts;
+
+ ts++;
+
+ return sampled;
+ }
+
+ bool init() override { return true; }
+
+ bool selfTest() override { return true; }
+};
+
+float minPosition = AeroBrakesConfigs::SERVO_MIN_POS;
+float maxPosition = AeroBrakesConfigs::SERVO_MAX_POS;
+float resetPosition = AeroBrakesConfigs::SERVO_MIN_POS;
+
+int main()
+{
+ sEventBroker->start();
+
+ miosix::GpioPin pwmPin = miosix::GpioPin(GPIOC_BASE, 7);
+ pwmPin.mode(miosix::Mode::ALTERNATE);
+ pwmPin.alternateFunction(3);
+
+ string temp;
+ for (;;)
+ {
+ int choice;
+ cout << "\n\nWhat do you want to do?:\n";
+ cout << "1. Aerobrakes algorithm simulation (with test data)\n";
+ cout << "2. Aerobrakes full extension\n";
+ cout << "3. Servo wiggle\n";
+ cout << "4. Servo fully open\n";
+ cout << "5. Servo fully closed\n";
+ cout << "6. Servo reset\n";
+ cout << "7. Servo manual control\n";
+ cout << "8. Set servo parameters\n";
+ cout << "9. Reset servo parameters (default)\n";
+
+ getline(cin, temp);
+ stringstream(temp) >> choice;
+
+ switch (choice)
+ {
+ case 1:
+ testAlgorithm();
+ break;
+ case 2:
+ testAeroBrakes();
+ break;
+ case 3:
+ wiggleServo();
+ break;
+ case 4:
+ setServoFullyOpen();
+ break;
+ case 5:
+ setServoFullyClosed();
+ break;
+ case 6:
+ resetServo();
+ break;
+ case 7:
+ manualServoControl();
+ break;
+ case 8:
+ setServoParameters();
+ break;
+ case 9:
+ resetServoParameters();
+ break;
+ default:
+ cout << "Invalid option \n";
+ break;
+ }
+ }
+}
+
+void testAlgorithm()
+{
+ cout << "\n\n** AEROBRAKES ALGORITHM SIMULATION **\n\n";
+
+ waitUserInput();
+
+ // AeroBrakesController initialization
+ MockSensor<InputClass> sensor;
+ AeroBrakesServo servo{minPosition, maxPosition, resetPosition};
+ AeroBrakesController<InputClass> aerobrakesController(sensor, &servo);
+
+ // Start the state machine
+ aerobrakesController.start();
+ sensor.sample();
+ EventCounter counter{*sEventBroker};
+ counter.subscribe(TOPIC_FLIGHT_EVENTS);
+
+ // Start test
+ cout << "Starting aerobrakes test\n";
+ sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS);
+ while (counter.getCount(EV_SHADOW_MODE_TIMEOUT) < 1)
+ {
+ Thread::sleep(10);
+ }
+
+ for (int i = 1; i < LEN_TEST; i++)
+ {
+ sensor.sample();
+ aerobrakesController.update();
+ cout << "z: " << sensor.getLastSample().z
+ << "\tvz: " << sensor.getLastSample().vz
+ << "\tvMod: " << sensor.getLastSample().vMod
+ << "\tservo: " << servo.getCurrentPosition() << "\n";
+ Thread::sleep(AeroBrakesConfigs::UPDATE_TIME);
+ }
+
+ aerobrakesController.stop();
+ counter.stop();
+
+ cout << "\n\tTest finished!\n";
+}
+
+void testAeroBrakes()
+{
+ cout << "\n\n** AEROBRAKES FULL EXTENSION **\n\n";
+
+ waitUserInput();
+
+ // AeroBrakesController initialization
+ MockSensor<InputClass> sensor{};
+ AeroBrakesServo servo{minPosition, maxPosition, resetPosition};
+ AeroBrakesController<InputClass> aerobrakesController{sensor, &servo};
+
+ // Start the state machine
+ aerobrakesController.start();
+ EventCounter counter{*sEventBroker};
+ counter.subscribe(TOPIC_ABK);
+
+ // Start test
+ cout << "Starting aerobrakes test\n";
+ sEventBroker->post({EV_TEST_ABK}, TOPIC_ABK);
+
+ while (counter.getCount(EV_TEST_TIMEOUT) < 1)
+ {
+ Thread::sleep(100);
+ }
+
+ aerobrakesController.stop();
+ counter.stop();
+
+ cout << "\n\tTest finished!\n";
+}
+
+void wiggleServo()
+{
+ string temp;
+ cout << "\n\n** WIGGLE SERVO **\n\n";
+
+ waitUserInput();
+
+ AeroBrakesServo servo{minPosition, maxPosition, resetPosition};
+ servo.enable();
+
+ cout << "Wiggling ... \n";
+ servo.selfTest();
+ Thread::sleep(10000);
+ servo.disable();
+
+ cout << "\n\tDone!\n";
+}
+
+void setServoFullyOpen()
+{
+ string temp;
+ cout << "\n\n** SERVO FULLY OPEN **\n\n";
+
+ waitUserInput();
+
+ AeroBrakesServo servo{minPosition, maxPosition, resetPosition};
+ servo.enable();
+ servo.setMaxPosition();
+ Thread::sleep(10000);
+ servo.disable();
+
+ cout << "\n\tDone!\n";
+}
+
+void setServoFullyClosed()
+{
+ string temp;
+ cout << "\n\n** SERVO FULLY CLOSED **\n\n";
+
+ waitUserInput();
+
+ AeroBrakesServo servo{minPosition, maxPosition, resetPosition};
+ servo.enable();
+ servo.setMinPosition();
+ Thread::sleep(10000);
+ servo.disable();
+
+ cout << "\n\tDone!\n";
+}
+
+void resetServo()
+{
+ string temp;
+ cout << "\n\n** RESET SERVO **\n\n";
+
+ waitUserInput();
+
+ AeroBrakesServo servo{minPosition, maxPosition, resetPosition};
+ servo.enable();
+ servo.reset();
+ Thread::sleep(1000);
+ servo.disable();
+
+ cout << "\n\tDone!\n";
+}
+
+void manualServoControl()
+{
+ cout << "\n\n** MANUAL SERVO CONTROL **\n\n";
+
+ string temp;
+ float angle;
+ do
+ {
+ cout << "Write the servo postion (degrees):\n";
+ getline(cin, temp);
+ stringstream(temp) >> angle;
+ } while (angle < minPosition || angle > maxPosition);
+
+ AeroBrakesServo servo{minPosition, maxPosition, resetPosition};
+
+ servo.enable();
+ while (servo.getCurrentPosition() != angle)
+ {
+ servo.set(angle);
+ Thread::sleep(AeroBrakesConfigs::UPDATE_TIME);
+ }
+ Thread::sleep(100);
+ servo.disable();
+
+ cout << "\n\tDone!\n";
+}
+
+void setServoParameters()
+{
+ cout << "\n\n** SET SERVO PARAMETERS **\n\n";
+
+ string temp;
+ do
+ {
+ cout << "Write the servo minimum postion (degrees):\n";
+ getline(cin, temp);
+ stringstream(temp) >> minPosition;
+ } while (minPosition < 0 || minPosition > 180.0f);
+
+ do
+ {
+ cout << "Write the servo maximum postion (degrees):\n";
+ getline(cin, temp);
+ stringstream(temp) >> maxPosition;
+ } while (maxPosition < 0 || maxPosition > 180.0f);
+
+ do
+ {
+ cout << "Write the servo reset postion (degrees):\n";
+ getline(cin, temp);
+ stringstream(temp) >> resetPosition;
+ } while (resetPosition < 0 || resetPosition > 180.0f);
+
+ cout << "Configured parameteres:\n";
+ cout << "\tminimum position: " << minPosition << "\n";
+ cout << "\tmaximum position: " << maxPosition << "\n";
+ cout << "\treset position: " << resetPosition << "\n";
+}
+
+void resetServoParameters()
+{
+ minPosition = AeroBrakesConfigs::SERVO_MIN_POS;
+ maxPosition = AeroBrakesConfigs::SERVO_MAX_POS;
+ resetPosition = AeroBrakesConfigs::SERVO_MIN_POS;
+
+ cout << "Configured parameteres (default):\n";
+ cout << "\tminimum position: " << minPosition << "\n";
+ cout << "\tmaximum position: " << maxPosition << "\n";
+ cout << "\treset position: " << resetPosition << "\n";
+}
+
+void waitUserInput()
+{
+ string temp;
+ do
+ {
+ cout << "Write 'start' to begin the test:\n";
+ getline(cin, temp);
+ } while (temp != "start");
+}
\ No newline at end of file
diff --git a/src/tests/aerobrakes/test_data.h b/src/tests/aerobrakes/test_data.h
new file mode 100644
index 0000000000000000000000000000000000000000..a937b6966b2a1d64f6bd56005868f40428773987
--- /dev/null
+++ b/src/tests/aerobrakes/test_data.h
@@ -0,0 +1,390 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Vincenzo Santomarco
+ *
+ * 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
+
+static const int LEN_TEST = 169;
+
+struct input_t
+{
+ float z;
+ float vz;
+ float vMod;
+ float sampleTime;
+};
+
+struct output_t
+{
+ float z_setpoint;
+ float vz_setpoint;
+ float u;
+ float deltas;
+ int alpha;
+};
+
+struct test_t
+{
+ input_t input;
+ output_t output;
+};
+
+const test_t DATA[LEN_TEST] = {
+ {{1407.523534, 223.918458, 232.831273, 0.1},
+ {1410.698478, 224.370516, 0.000000, 0.000000, 0}},
+ {{1429.826222, 222.137686, 231.046739, 0.1},
+ {1433.007837, 221.822602, 22.001227, 0.000000, 0}},
+ {{1451.951540, 220.370997, 229.276914, 0.1},
+ {1455.064742, 219.321178, 83.824886, 0.000000, 0}},
+ {{1473.900883, 218.618135, 227.521532, 0.1},
+ {1476.873741, 216.864257, 146.806876, 0.000000, 0}},
+ {{1495.675620, 216.878841, 225.780334, 0.1},
+ {1498.439192, 214.449971, 210.925533, 0.002500, 8}},
+ {{1517.277018, 215.150907, 224.051216, 0.1},
+ {1519.765268, 212.076566, 275.998544, 0.004500, 15}},
+ {{1538.692467, 213.164813, 222.050419, 0.1},
+ {1540.855976, 209.742391, 319.912919, 0.006000, 21}},
+ {{1559.896941, 210.929504, 219.786565, 0.1},
+ {1561.715159, 207.445889, 342.042809, 0.007000, 26}},
+ {{1580.868317, 208.503942, 217.321830, 0.1},
+ {1582.346512, 205.185595, 345.908908, 0.007000, 26}},
+ {{1601.589631, 205.929325, 214.699801, 0.1},
+ {1602.753584, 202.960124, 333.870736, 0.007000, 26}},
+ {{1622.055564, 203.396178, 212.121054, 0.1},
+ {1622.939793, 200.768167, 320.739091, 0.007000, 26}},
+ {{1642.270215, 200.903468, 209.584307, 0.1},
+ {1642.908428, 198.608488, 306.570638, 0.007000, 26}},
+ {{1662.237560, 198.449839, 207.088281, 0.1},
+ {1662.662657, 196.479916, 291.390833, 0.007000, 26}},
+ {{1681.961982, 196.058211, 204.657393, 0.1},
+ {1682.205536, 194.381342, 277.209978, 0.006500, 23}},
+ {{1701.451202, 193.731747, 202.294942, 0.1},
+ {1701.540011, 192.311716, 264.533577, 0.006500, 23}},
+ {{1720.712684, 191.503026, 200.035297, 0.1},
+ {1720.668927, 190.270041, 256.296072, 0.006500, 23}},
+ {{1739.752816, 189.304576, 197.807258, 0.1},
+ {1739.595032, 188.255368, 247.391254, 0.006500, 23}},
+ {{1758.574577, 187.135466, 195.609965, 0.1},
+ {1758.320980, 186.266798, 237.832929, 0.006000, 21}},
+ {{1777.180861, 184.994918, 193.442602, 0.1},
+ {1776.849338, 184.303477, 227.643718, 0.006000, 21}},
+ {{1795.578146, 182.954926, 191.381404, 0.1},
+ {1795.182591, 182.364588, 222.810466, 0.006000, 21}},
+ {{1813.772666, 180.939526, 189.345943, 0.1},
+ {1813.323143, 180.449357, 217.548324, 0.006000, 21}},
+ {{1831.767240, 178.960146, 187.348411, 0.1},
+ {1831.273322, 178.557045, 212.859568, 0.006000, 21}},
+ {{1849.565570, 177.010170, 185.381850, 0.1},
+ {1849.035385, 176.686946, 208.325170, 0.006000, 21}},
+ {{1867.170012, 175.082317, 183.438539, 0.1},
+ {1866.611519, 174.838389, 203.439009, 0.006000, 21}},
+ {{1884.582753, 173.176068, 181.517929, 0.1},
+ {1884.003846, 173.010730, 198.214336, 0.006000, 21}},
+ {{1901.805928, 171.290886, 179.619473, 0.1},
+ {1901.214426, 171.203355, 192.660780, 0.006000, 21}},
+ {{1918.841611, 169.426147, 177.742548, 0.1},
+ {1918.245256, 169.415678, 186.779343, 0.006000, 21}},
+ {{1935.691826, 167.581464, 175.886774, 0.1},
+ {1935.098279, 167.647137, 180.588101, 0.006000, 21}},
+ {{1952.358556, 165.756368, 174.051669, 0.1},
+ {1951.775381, 165.897193, 174.097177, 0.006000, 21}},
+ {{1968.843737, 163.950404, 172.236762, 0.1},
+ {1968.278396, 164.165332, 167.316651, 0.006000, 21}},
+ {{1985.149439, 162.169295, 170.448186, 0.1},
+ {1984.609107, 162.451059, 160.761439, 0.005500, 19}},
+ {{2001.278287, 160.410614, 168.683383, 0.1},
+ {2000.769249, 160.753900, 154.307788, 0.005500, 19}},
+ {{2017.233572, 158.697809, 166.967443, 0.1},
+ {2016.760511, 159.073401, 149.942314, 0.005500, 19}},
+ {{2033.018389, 157.001195, 165.268700, 0.1},
+ {2032.584538, 157.409124, 145.412723, 0.005500, 19}},
+ {{2048.634338, 155.320412, 163.586791, 0.1},
+ {2048.242929, 155.760649, 140.723784, 0.005500, 19}},
+ {{2064.082987, 153.655113, 161.921368, 0.1},
+ {2063.737245, 154.127573, 135.880349, 0.005000, 17}},
+ {{2079.365866, 152.004960, 160.272087, 0.1},
+ {2079.069006, 152.509506, 130.886994, 0.005000, 17}},
+ {{2094.486432, 150.408612, 158.680445, 0.1},
+ {2094.239695, 150.906075, 128.945069, 0.005000, 17}},
+ {{2109.448027, 148.825441, 157.102924, 0.1},
+ {2109.250756, 149.316919, 126.948556, 0.005000, 17}},
+ {{2124.251954, 147.255236, 155.539324, 0.1},
+ {2124.103601, 147.741691, 124.903075, 0.005000, 17}},
+ {{2138.899517, 145.698979, 153.990723, 0.1},
+ {2138.799604, 146.180055, 122.911826, 0.005500, 19}},
+ {{2153.392272, 144.158165, 152.458755, 0.1},
+ {2153.340110, 144.631690, 121.125681, 0.005500, 19}},
+ {{2167.729859, 142.595759, 150.903540, 0.1},
+ {2167.726428, 143.096281, 116.544249, 0.005000, 17}},
+ {{2181.911853, 141.046238, 149.362216, 0.1},
+ {2181.959840, 141.573528, 111.846697, 0.005000, 17}},
+ {{2195.941149, 139.541589, 147.869341, 0.1},
+ {2196.041597, 140.063140, 109.680869, 0.005000, 17}},
+ {{2209.820551, 138.048329, 146.388841, 0.1},
+ {2209.972921, 138.564834, 107.486848, 0.005000, 17}},
+ {{2223.551186, 136.566227, 144.920493, 0.1},
+ {2223.755007, 137.078337, 105.264693, 0.005000, 17}},
+ {{2237.134161, 135.095065, 143.464081, 0.1},
+ {2237.389022, 135.603386, 103.014797, 0.005000, 17}},
+ {{2250.570557, 133.634627, 142.019394, 0.1},
+ {2250.876108, 134.139726, 100.737494, 0.005000, 17}},
+ {{2263.861436, 132.184701, 140.586228, 0.1},
+ {2264.217381, 132.687107, 98.432779, 0.005000, 17}},
+ {{2277.007840, 130.745074, 139.164381, 0.1},
+ {2277.413934, 131.245290, 96.100334, 0.005000, 17}},
+ {{2290.010788, 129.315539, 137.753660, 0.1},
+ {2290.466835, 129.814042, 93.739752, 0.005000, 17}},
+ {{2302.871343, 127.897638, 136.355777, 0.1},
+ {2303.377130, 128.393136, 91.493595, 0.005000, 17}},
+ {{2315.590644, 126.489974, 134.969234, 0.1},
+ {2316.145842, 126.982353, 89.271851, 0.005000, 17}},
+ {{2328.169653, 125.091761, 133.593199, 0.1},
+ {2328.773972, 125.581479, 87.028189, 0.005000, 17}},
+ {{2340.609305, 123.702814, 132.227500, 0.1},
+ {2341.262502, 124.190307, 84.762076, 0.005000, 17}},
+ {{2352.910518, 122.322948, 130.871970, 0.1},
+ {2353.612390, 122.808635, 82.472658, 0.005000, 17}},
+ {{2365.074191, 120.951981, 129.526447, 0.1},
+ {2365.824578, 121.436267, 80.159115, 0.005000, 17}},
+ {{2377.101205, 119.589741, 128.190773, 0.1},
+ {2377.899985, 120.073012, 77.820896, 0.005000, 17}},
+ {{2388.992425, 118.236061, 126.864795, 0.1},
+ {2389.839515, 118.718683, 75.457721, 0.004500, 15}},
+ {{2400.748697, 116.890780, 125.548366, 0.1},
+ {2401.644050, 117.373101, 73.069412, 0.004500, 15}},
+ {{2412.371612, 115.568806, 124.257936, 0.1},
+ {2413.314456, 116.036087, 71.891017, 0.004500, 15}},
+ {{2423.862714, 114.254488, 122.976329, 0.1},
+ {2424.851582, 114.707470, 70.727132, 0.005000, 17}},
+ {{2435.222760, 112.947681, 121.703422, 0.1},
+ {2436.256259, 113.387082, 69.575894, 0.005000, 17}},
+ {{2446.451788, 111.634162, 120.423512, 0.1},
+ {2447.529301, 112.074759, 67.280809, 0.004500, 15}},
+ {{2457.549877, 110.328894, 119.153172, 0.1},
+ {2458.671507, 110.770342, 65.008026, 0.004500, 15}},
+ {{2468.518481, 109.044366, 117.906294, 0.1},
+ {2469.683660, 109.473673, 63.796332, 0.004500, 15}},
+ {{2479.358982, 107.766798, 116.667643, 0.1},
+ {2480.566527, 108.184601, 62.593195, 0.004500, 15}},
+ {{2490.072068, 106.496060, 115.437113, 0.1},
+ {2491.320859, 106.902976, 61.396891, 0.005000, 17}},
+ {{2500.658417, 105.231983, 114.214575, 0.1},
+ {2501.947396, 105.628652, 60.202504, 0.005000, 17}},
+ {{2511.118055, 103.961930, 112.985948, 0.1},
+ {2512.446858, 104.361488, 57.982258, 0.004500, 15}},
+ {{2521.451029, 102.698677, 111.765518, 0.1},
+ {2522.819957, 103.101343, 55.729628, 0.004500, 15}},
+ {{2531.658591, 101.453613, 110.566106, 0.1},
+ {2533.067385, 101.848081, 54.388563, 0.004500, 15}},
+ {{2541.741959, 100.214775, 109.374353, 0.1},
+ {2543.189826, 100.601569, 53.045474, 0.004500, 15}},
+ {{2551.701750, 98.982052, 108.190179, 0.1},
+ {2553.187947, 99.361675, 51.699514, 0.004500, 15}},
+ {{2561.538570, 97.755336, 107.013501, 0.1},
+ {2563.062404, 98.128272, 50.349771, 0.004500, 15}},
+ {{2571.253014, 96.534517, 105.844243, 0.1},
+ {2572.813840, 96.901233, 48.995142, 0.004500, 15}},
+ {{2580.845666, 95.319487, 104.682332, 0.1},
+ {2582.442885, 95.680437, 47.634353, 0.004500, 15}},
+ {{2590.317100, 94.110134, 103.527695, 0.1},
+ {2591.950157, 94.465761, 46.266054, 0.004500, 15}},
+ {{2599.667867, 92.906047, 102.379915, 0.1},
+ {2601.336262, 93.257089, 44.863988, 0.004500, 15}},
+ {{2608.898492, 91.707370, 101.239199, 0.1},
+ {2610.601795, 92.054303, 43.445653, 0.004000, 13}},
+ {{2618.009521, 90.514092, 100.105582, 0.1},
+ {2619.747339, 90.857290, 42.017225, 0.004000, 13}},
+ {{2627.002040, 89.337126, 98.991641, 0.1},
+ {2628.773466, 89.665938, 41.480883, 0.004000, 13}},
+ {{2635.877108, 88.165040, 97.884362, 0.1},
+ {2637.680735, 88.480138, 40.961420, 0.004500, 15}},
+ {{2644.635208, 86.997737, 96.783700, 0.1},
+ {2646.469697, 87.299782, 40.456275, 0.004500, 15}},
+ {{2653.276298, 85.824893, 95.677786, 0.1},
+ {2655.140892, 86.124763, 39.124360, 0.004500, 15}},
+ {{2661.800350, 84.656946, 94.578723, 0.1},
+ {2663.694846, 84.954979, 37.775662, 0.004500, 15}},
+ {{2670.207848, 83.493807, 93.486471, 0.1},
+ {2672.132080, 83.790326, 36.409646, 0.004000, 13}},
+ {{2678.499269, 82.335394, 92.400999, 0.1},
+ {2680.453100, 82.630705, 35.026106, 0.004000, 13}},
+ {{2686.675545, 81.190866, 91.333090, 0.1},
+ {2688.658406, 81.476016, 34.382681, 0.004000, 13}},
+ {{2694.737585, 80.050628, 90.271622, 0.1},
+ {2696.748485, 80.326164, 33.745383, 0.004000, 13}},
+ {{2702.685811, 78.914571, 89.216537, 0.1},
+ {2700.750465, 79.753021, 0.000000, 0.000000, 0}},
+ {{2710.520635, 77.782587, 88.167786, 0.1},
+ {2708.668599, 78.610244, 0.000000, 0.000000, 0}},
+ {{2718.244674, 76.698652, 87.177631, 0.1},
+ {2716.472686, 77.472068, 0.000000, 0.000000, 0}},
+ {{2725.860456, 75.617439, 86.192585, 0.1},
+ {2724.163181, 76.338400, 0.000000, 0.000000, 0}},
+ {{2733.368175, 74.537122, 85.210554, 0.1},
+ {2731.740532, 75.209151, 0.000000, 0.000000, 0}},
+ {{2740.767967, 73.459160, 84.233359, 0.1},
+ {2739.205174, 74.084232, 2.638818, 0.000000, 0}},
+ {{2748.060095, 72.383840, 83.261426, 0.1},
+ {2746.557537, 72.963557, 3.232653, 0.000000, 0}},
+ {{2755.244822, 71.311123, 82.294793, 0.1},
+ {2753.798041, 71.847038, 3.925782, 0.000000, 0}},
+ {{2762.322406, 70.240971, 81.333504, 0.1},
+ {2760.927097, 70.734592, 4.714279, 0.000000, 0}},
+ {{2769.293100, 69.173345, 80.377603, 0.1},
+ {2767.945109, 69.626135, 5.594345, 0.000000, 0}},
+ {{2776.157158, 68.108208, 79.427143, 0.1},
+ {2774.852471, 68.521586, 6.562208, 0.000000, 0}},
+ {{2782.914824, 67.045519, 78.482177, 0.1},
+ {2781.649570, 67.420863, 7.614075, 0.000000, 0}},
+ {{2789.566341, 65.985236, 77.542764, 0.1},
+ {2788.336784, 66.323887, 8.746139, 0.000000, 0}},
+ {{2796.111950, 64.927316, 76.608969, 0.1},
+ {2794.914485, 65.230580, 9.954627, 0.000000, 0}},
+ {{2802.551882, 63.871717, 75.680859, 0.1},
+ {2801.383035, 64.140865, 11.235865, 0.000000, 0}},
+ {{2808.886369, 62.818398, 74.758508, 0.1},
+ {2807.742790, 63.054665, 12.586344, 0.000000, 0}},
+ {{2815.115636, 61.767318, 73.841996, 0.1},
+ {2813.994097, 61.971905, 14.002770, 0.000000, 0}},
+ {{2821.239906, 60.718440, 72.931408, 0.1},
+ {2820.137297, 60.892512, 15.482087, 0.001000, 3}},
+ {{2827.259396, 59.671727, 72.026834, 0.1},
+ {2826.172723, 59.816412, 17.021470, 0.002500, 8}},
+ {{2833.174044, 58.621604, 71.121232, 0.1},
+ {2832.100700, 58.743534, 18.163957, 0.003500, 11}},
+ {{2838.983308, 57.564111, 70.209511, 0.1},
+ {2837.921548, 57.673807, 18.557488, 0.004000, 13}},
+ {{2844.686607, 56.502319, 69.295655, 0.1},
+ {2843.635577, 56.607161, 18.407003, 0.004500, 15}},
+ {{2850.283663, 55.439136, 68.383536, 0.1},
+ {2849.243093, 55.543527, 17.919713, 0.004500, 15}},
+ {{2855.774270, 54.373463, 67.471945, 0.1},
+ {2854.744393, 54.482838, 16.989111, 0.004000, 13}},
+ {{2861.158442, 53.310395, 66.567992, 0.1},
+ {2860.139768, 53.425025, 16.011311, 0.004000, 13}},
+ {{2866.436640, 52.253928, 65.677398, 0.1},
+ {2865.429503, 52.370024, 15.317990, 0.004000, 13}},
+ {{2871.609301, 51.199672, 64.794534, 0.1},
+ {2870.613876, 51.317768, 14.573491, 0.003500, 11}},
+ {{2876.676648, 50.147640, 63.919552, 0.1},
+ {2875.693158, 50.268194, 13.781458, 0.003500, 11}},
+ {{2881.639061, 49.100977, 63.056873, 0.1},
+ {2880.667613, 49.221238, 13.202782, 0.003000, 9}},
+ {{2886.496920, 48.056611, 62.202318, 0.1},
+ {2885.537501, 48.176836, 12.604339, 0.003000, 9}},
+ {{2891.250636, 47.018088, 61.360933, 0.1},
+ {2890.303074, 47.134928, 12.280825, 0.003000, 9}},
+ {{2895.900617, 45.981928, 60.527937, 0.1},
+ {2894.964578, 46.095452, 11.968529, 0.003500, 11}},
+ {{2900.447101, 44.948158, 59.703518, 0.1},
+ {2899.522253, 45.058347, 11.674383, 0.003500, 11}},
+ {{2904.890167, 43.913570, 58.883223, 0.1},
+ {2903.976334, 44.023555, 11.140163, 0.003500, 11}},
+ {{2909.229897, 42.881424, 58.072049, 0.1},
+ {2908.327049, 42.991016, 10.622481, 0.003000, 9}},
+ {{2913.466530, 41.851616, 57.270212, 0.1},
+ {2912.574619, 41.960672, 10.118410, 0.003000, 9}},
+ {{2917.600434, 40.826803, 56.482155, 0.1},
+ {2916.719263, 40.932466, 9.851340, 0.003000, 9}},
+ {{2921.631953, 39.803918, 55.703792, 0.1},
+ {2920.761191, 39.906341, 9.588696, 0.003500, 11}},
+ {{2925.561287, 38.783108, 54.935412, 0.1},
+ {2924.700607, 38.882242, 9.346359, 0.003500, 11}},
+ {{2929.388524, 37.762009, 54.173486, 0.1},
+ {2928.537713, 37.860112, 8.935239, 0.003500, 11}},
+ {{2933.113764, 36.743153, 53.422213, 0.1},
+ {2932.272702, 36.839897, 8.556173, 0.003500, 11}},
+ {{2936.737228, 35.726506, 52.681892, 0.1},
+ {2935.905762, 35.821543, 8.212388, 0.003500, 11}},
+ {{2940.259136, 34.711992, 51.952833, 0.1},
+ {2939.437078, 34.804997, 7.903881, 0.003500, 11}},
+ {{2943.679695, 33.699509, 51.235358, 0.1},
+ {2942.866827, 33.790206, 7.628086, 0.003500, 11}},
+ {{2946.999101, 32.688846, 50.529650, 0.1},
+ {2946.195183, 32.777117, 7.373489, 0.003500, 11}},
+ {{2950.217493, 31.679292, 49.835035, 0.1},
+ {2950.997969, 31.260565, 12.867386, 0.010000, 41}},
+ {{2953.335016, 30.671441, 49.153096, 0.1},
+ {2954.073563, 30.251509, 12.283864, 0.010000, 48}},
+ {{2956.350647, 29.641635, 48.441400, 0.1},
+ {2957.048328, 29.243979, 11.696148, 0.010000, 48}},
+ {{2959.263303, 28.611932, 47.739490, 0.1},
+ {2959.922414, 28.237924, 11.123138, 0.010000, 48}},
+ {{2962.073123, 27.584913, 47.052590, 0.1},
+ {2962.695966, 27.233296, 10.566719, 0.010000, 48}},
+ {{2964.780373, 26.560521, 46.381113, 0.1},
+ {2965.369125, 26.230046, 10.026509, 0.010000, 48}},
+ {{2967.385314, 25.538714, 45.725492, 0.1},
+ {2967.942025, 25.228128, 9.502122, 0.010000, 48}},
+ {{2969.888201, 24.519460, 45.086174, 0.1},
+ {2970.414798, 24.227493, 8.993173, 0.010000, 48}},
+ {{2972.289290, 23.502736, 44.463621, 0.1},
+ {2972.787570, 23.228095, 8.499271, 0.010000, 48}},
+ {{2974.588832, 22.488518, 43.858309, 0.1},
+ {2975.060462, 22.229890, 8.020013, 0.010000, 48}},
+ {{2976.787076, 21.476777, 43.270725, 0.1},
+ {2977.233591, 21.232831, 7.554990, 0.010000, 48}},
+ {{2978.884269, 20.467481, 42.701369, 0.1},
+ {2979.307070, 20.236874, 7.103775, 0.010000, 48}},
+ {{2980.880652, 19.460587, 42.150748, 0.1},
+ {2981.281006, 19.241976, 6.665932, 0.010000, 48}},
+ {{2982.776464, 18.456044, 41.619378, 0.1},
+ {2983.155503, 18.248095, 6.241007, 0.010000, 48}},
+ {{2984.571937, 17.453792, 41.107781, 0.1},
+ {2984.930661, 17.255187, 5.828534, 0.010000, 48}},
+ {{2986.267297, 16.453765, 40.616482, 0.1},
+ {2986.606575, 16.263212, 5.428031, 0.010000, 48}},
+ {{2987.862762, 15.455888, 40.146011, 0.1},
+ {2988.183337, 15.272130, 5.039001, 0.010000, 48}},
+ {{2989.358540, 14.460001, 39.696907, 0.1},
+ {2989.661033, 14.281903, 4.660909, 0.010000, 48}},
+ {{2990.754830, 13.466130, 39.269688, 0.1},
+ {2991.039748, 13.292492, 4.293264, 0.010000, 48}},
+ {{2992.051832, 12.474233, 38.864869, 0.1},
+ {2992.319561, 12.303862, 3.935539, 0.010000, 48}},
+ {{2993.249740, 11.484250, 38.482958, 0.1},
+ {2993.500549, 11.315977, 3.587188, 0.010000, 48}},
+ {{2994.348743, 10.496116, 38.124445, 0.1},
+ {2994.582783, 10.328804, 3.247651, 0.010000, 48}},
+ {{2995.349023, 9.509767, 37.789806, 0.1},
+ {2995.566335, 9.342312, 2.916352, 0.010000, 48}},
+ {{2996.250754, 8.525139, 37.479491, 0.1},
+ {2996.451270, 8.356472, 2.592704, 0.010000, 48}},
+ {{2997.054106, 7.542171, 37.193927, 0.1},
+ {2997.237653, 7.371257, 2.276107, 0.010000, 48}},
+ {{2997.759242, 6.560805, 36.933508, 0.1},
+ {2997.593906, 6.878876, 0.000000, 0.000000, 18}},
+ {{2998.366319, 5.580987, 36.698594, 0.1},
+ {2998.232574, 5.894551, 0.000000, 0.000000, 0}},
+ {{2998.875647, 4.605733, 36.513507, 0.1},
+ {2998.772838, 4.910794, 0.000000, 0.000000, 0}},
+ {{2999.287586, 3.633139, 36.370710, 0.1},
+ {2999.214753, 3.927587, 0.000000, 0.000000, 0}},
+ {{2999.602292, 2.661052, 36.253879, 0.1},
+ {2999.558375, 2.944913, 0.000000, 0.000000, 0}},
+ {{2999.819812, 1.689438, 36.163201, 0.1},
+ {2999.803756, 1.962763, 0.000000, 0.000000, 0}},
+ {{2999.940194, 0.718271, 36.098808, 0.1},
+ {2999.950947, 0.981127, 0.000000, 0.000000, 0}},
+ {{2999.963481, -0.252449, 36.060779, 0.1},
+ {2999.950947, 0.981127, 0.000000, 0.000000, 0}},
+};
diff --git a/src/tests/catch/ada/ada_kalman_p/test-ada-data.cpp b/src/tests/catch/ada/ada_kalman_p/test-ada-data.cpp
index 940039cbb98f0f341d0e0fa80c36a39c46e05505..a9494b8152dc786ead418819a4762645313df10c 100644
--- a/src/tests/catch/ada/ada_kalman_p/test-ada-data.cpp
+++ b/src/tests/catch/ada/ada_kalman_p/test-ada-data.cpp
@@ -22,7 +22,7 @@
*/
#include "test-ada-data.h"
-const float SIMULATED_PRESSURE[DATA_SIZE] = {
+const float ADA_SIMULATED_PRESSURE[DATA_SIZE] = {
85431.242948f, 85431.195530f, 85430.531818f, 85428.511661f, 85424.541375f, 85418.586756f, 85410.484742f, 85400.380605f, 85387.951291f, 85373.412847f, 85356.530561f, 85337.302634f, 85315.693843f, 85291.654118f, 85265.156486f, 85236.179384f, 85204.699993f, 85170.715288f, 85134.214699f, 85095.204539f, 85053.713878f, 85009.692280f, 84963.342580f, 84914.537170f, 84863.342981f, 84809.942661f, 84754.307845f, 84696.606779f, 84636.840947f, 84575.108256f, 84511.411803f, 84445.886248f, 84378.531812f, 84309.408539f, 84238.575457f, 84166.083903f, 84091.993574f, 84016.326350f, 83939.283495f, 83860.838930f, 83781.113349f, 83700.192751f, 83618.160747f, 83535.097047f, 83451.069870f, 83366.139284f, 83280.348743f, 83193.731035f, 83106.324146f, 83018.168763f,
82929.332706f, 82839.907721f, 82749.980821f, 82659.659037f, 82569.047818f, 82478.250367f, 82387.372687f, 82296.514121f, 82205.765806f, 82115.204232f, 82024.866544f, 81934.773692f, 81844.948233f, 81755.416882f, 81666.211710f, 81577.355814f, 81488.856279f, 81400.744909f, 81313.024685f, 81225.725720f, 81138.858951f, 81052.432671f, 80966.442916f, 80880.879969f, 80795.733506f, 80710.993461f, 80626.652792f, 80542.709300f, 80459.154952f, 80376.001162f, 80293.249194f, 80210.902104f, 80128.967362f, 80047.445669f, 79966.333243f, 79885.651990f, 79805.357504f, 79725.473811f, 79645.974136f, 79566.865375f, 79488.136895f, 79409.786770f, 79331.813688f, 79254.217678f, 79177.018164f, 79100.171172f, 79023.724092f, 78947.664215f, 78871.990160f, 78796.711133f,
78721.810653f, 78647.294473f, 78573.155658f, 78499.389033f, 78425.989137f, 78352.953270f, 78280.274531f, 78207.954674f, 78135.994226f, 78064.391522f, 77993.148588f, 77922.267607f, 77851.747240f, 77781.592871f, 77711.801941f, 77642.368219f, 77573.321504f, 77504.601044f, 77436.249004f, 77368.249493f, 77300.601718f, 77233.292329f, 77166.332527f, 77099.709056f, 77033.429981f, 76967.488574f, 76901.887521f, 76836.627276f, 76771.707698f, 76707.131456f, 76642.893513f, 76578.999459f, 76515.448480f, 76452.236220f, 76389.362923f, 76326.825701f, 76264.622110f, 76202.749523f, 76141.205158f, 76079.986697f, 76019.092066f, 75958.519777f, 75898.268885f, 75838.338743f, 75778.729023f, 75719.439688f, 75660.470760f, 75601.822193f, 75543.497084f, 75485.490333f,
@@ -122,7 +122,7 @@ const float SIMULATED_PRESSURE[DATA_SIZE] = {
84580.016493f, 84580.873337f, 84581.729482f, 84582.585353f, 84583.441370f, 84584.297956f, 84585.155471f, 84586.013678f, 84586.872320f, 84587.731198f, 84588.590112f, 84589.448860f, 84590.307243f, 84591.165128f, 84592.022639f, 84592.879929f, 84593.737152f, 84594.594460f, 84595.452004f, 84596.309938f, 84597.168345f, 84598.027100f, 84598.886042f, 84599.745009f, 84600.603838f, 84601.462368f, 84602.320435f, 84603.178003f, 84604.035255f, 84604.892387f, 84605.749595f, 84606.607077f, 84607.465029f, 84608.324161f, 84609.184816f, 84610.046054f, 84610.906919f, 84611.766456f, 84612.623711f, 84613.477537
};
-const float SIMULATED_PRESSURE_SPEED[] = {
+const float ADA_SIMULATED_PRESSURE_SPEED[] = {
-0.948363f, -0.948363f, -13.274254f, -40.403141f, -79.405710f, -119.092389f, -162.040265f, -202.082736f, -248.586283f, -290.768883f, -337.645715f, -384.558541f, -432.175827f, -480.794502f, -529.952635f, -579.542043f, -629.587822f, -679.694102f, -730.011778f, -780.203192f, -829.813236f, -880.431945f, -926.994001f, -976.108202f, -1023.883786f, -1068.006391f, -1112.696323f, -1154.021320f, -1195.316651f, -1234.653809f, -1273.929067f, -1310.511099f, -1347.088722f, -1382.465466f, -1416.661632f, -1449.831084f, -1481.806571f, -1513.344488f, -1540.857087f, -1568.891304f, -1594.511632f, -1618.411951f, -1640.640080f, -1661.274003f, -1680.543531f, -1698.611728f, -1715.810816f, -1732.354168f, -1748.137780f, -1763.107652f,
-1776.721145f, -1788.499697f, -1798.538005f, -1806.435685f, -1812.224381f, -1815.949022f, -1817.553582f, -1817.171319f, -1814.966301f, -1811.231493f, -1806.753756f, -1801.857044f, -1796.509169f, -1790.627019f, -1784.103445f, -1777.117924f, -1769.990709f, -1762.227380f, -1754.404480f, -1745.979311f, -1737.335373f, -1728.525613f, -1719.795091f, -1711.258934f, -1702.929259f, -1694.800914f, -1686.813373f, -1678.869836f, -1671.086967f, -1663.075796f, -1655.039355f, -1646.941808f, -1638.694844f, -1630.433859f, -1622.248517f, -1613.625054f, -1605.889735f, -1597.673857f, -1589.993489f, -1582.175234f, -1574.569597f, -1567.002497f, -1559.461641f, -1551.920197f, -1543.990283f, -1536.939837f, -1528.941596f, -1521.197540f, -1513.481103f, -1505.580544f,
-1498.009587f, -1490.323611f, -1482.776293f, -1475.332504f, -1467.997921f, -1460.717334f, -1453.574783f, -1446.397141f, -1439.208953f, -1432.054093f, -1424.858679f, -1417.619623f, -1410.407338f, -1403.087371f, -1395.818595f, -1388.674458f, -1380.934283f, -1374.409215f, -1367.040802f, -1359.990206f, -1352.955497f, -1346.187796f, -1339.196030f, -1332.469424f, -1325.581498f, -1318.828149f, -1312.021043f, -1305.204915f, -1298.391547f, -1291.524853f, -1284.758862f, -1277.881065f, -1271.019585f, -1264.245204f, -1257.465943f, -1250.744429f, -1244.071819f, -1237.451756f, -1230.887296f, -1224.369212f, -1217.892631f, -1211.445773f, -1205.017845f, -1198.602846f, -1192.194381f, -1185.786708f, -1179.378571f, -1172.971336f, -1166.502184f, -1160.135006f,
@@ -223,7 +223,7 @@ const float SIMULATED_PRESSURE_SPEED[] = {
};
/*
-const float SIMULATED_NOISY_PRESSURE[DATA_SIZE] = {85440.0f, 85440.0f, 85410.0f, 85440.0f, 85440.0f, 85410.0f, 85410.0f, 85410.0f, 85380.0f, 85380.0f, 85350.0f, 85350.0f, 85320.0f, 85290.0f, 85260.0f, 85230.0f, 85200.0f, 85170.0f, 85140.0f, 85110.0f, 85050.0f, 85020.0f, 84960.0f, 84930.0f, 84870.0f, 84810.0f, 84750.0f, 84690.0f, 84630.0f, 84570.0f, 84510.0f, 84450.0f, 84390.0f, 84300.0f, 84240.0f, 84150.0f, 84090.0f, 84030.0f, 83940.0f, 83850.0f, 83790.0f, 83700.0f, 83610.0f, 83520.0f, 83460.0f, 83370.0f, 83280.0f, 83190.0f, 83100.0f, 83010.0f,
+const float ADA_SIMULATED_NOISY_PRESSURE[DATA_SIZE] = {85440.0f, 85440.0f, 85410.0f, 85440.0f, 85440.0f, 85410.0f, 85410.0f, 85410.0f, 85380.0f, 85380.0f, 85350.0f, 85350.0f, 85320.0f, 85290.0f, 85260.0f, 85230.0f, 85200.0f, 85170.0f, 85140.0f, 85110.0f, 85050.0f, 85020.0f, 84960.0f, 84930.0f, 84870.0f, 84810.0f, 84750.0f, 84690.0f, 84630.0f, 84570.0f, 84510.0f, 84450.0f, 84390.0f, 84300.0f, 84240.0f, 84150.0f, 84090.0f, 84030.0f, 83940.0f, 83850.0f, 83790.0f, 83700.0f, 83610.0f, 83520.0f, 83460.0f, 83370.0f, 83280.0f, 83190.0f, 83100.0f, 83010.0f,
82920.0f, 82860.0f, 82740.0f, 82650.0f, 82560.0f, 82470.0f, 82380.0f, 82290.0f, 82200.0f, 82110.0f, 82020.0f, 81930.0f, 81840.0f, 81750.0f, 81660.0f, 81570.0f, 81510.0f, 81390.0f, 81300.0f, 81240.0f, 81120.0f, 81060.0f, 80970.0f, 80880.0f, 80790.0f, 80700.0f, 80640.0f, 80550.0f, 80460.0f, 80370.0f, 80280.0f, 80220.0f, 80130.0f, 80040.0f, 79980.0f, 79890.0f, 79800.0f, 79710.0f, 79650.0f, 79560.0f, 79500.0f, 79410.0f, 79320.0f, 79260.0f, 79170.0f, 79110.0f, 79020.0f, 78930.0f, 78870.0f, 78810.0f,
78720.0f, 78630.0f, 78570.0f, 78510.0f, 78420.0f, 78360.0f, 78300.0f, 78210.0f, 78120.0f, 78060.0f, 78000.0f, 77940.0f, 77850.0f, 77790.0f, 77700.0f, 77640.0f, 77580.0f, 77490.0f, 77430.0f, 77370.0f, 77310.0f, 77250.0f, 77160.0f, 77100.0f, 77040.0f, 76950.0f, 76920.0f, 76830.0f, 76770.0f, 76710.0f, 76650.0f, 76590.0f, 76530.0f, 76440.0f, 76380.0f, 76320.0f, 76260.0f, 76200.0f, 76140.0f, 76080.0f, 76020.0f, 75960.0f, 75900.0f, 75840.0f, 75780.0f, 75720.0f, 75660.0f, 75600.0f, 75540.0f, 75480.0f,
75420.0f, 75360.0f, 75330.0f, 75270.0f, 75210.0f, 75150.0f, 75090.0f, 75030.0f, 74970.0f, 74910.0f, 74880.0f, 74820.0f, 74760.0f, 74700.0f, 74670.0f, 74610.0f, 74550.0f, 74490.0f, 74460.0f, 74400.0f, 74340.0f, 74280.0f, 74220.0f, 74190.0f, 74130.0f, 74070.0f, 74040.0f, 73980.0f, 73920.0f, 73890.0f, 73830.0f, 73800.0f, 73740.0f, 73710.0f, 73650.0f, 73590.0f, 73560.0f, 73500.0f, 73470.0f, 73410.0f, 73350.0f, 73320.0f, 73260.0f, 73230.0f, 73200.0f, 73140.0f, 73110.0f, 73050.0f, 73020.0f, 72990.0f,
diff --git a/src/tests/catch/ada/ada_kalman_p/test-ada-data.h b/src/tests/catch/ada/ada_kalman_p/test-ada-data.h
index 69d8d8a238651aa0f375ecd68e1fa676af675af1..1737bd7528e42cd2517e3df11f41fe390b3125db 100644
--- a/src/tests/catch/ada/ada_kalman_p/test-ada-data.h
+++ b/src/tests/catch/ada/ada_kalman_p/test-ada-data.h
@@ -24,6 +24,6 @@
#pragma once
static const unsigned DATA_SIZE = 4840;
-extern const float SIMULATED_PRESSURE[DATA_SIZE];
-extern const float SIMULATED_PRESSURE_SPEED[DATA_SIZE];
-// extern const float SIMULATED_NOISY_PRESSURE[DATA_SIZE];
\ No newline at end of file
+extern const float ADA_SIMULATED_PRESSURE[DATA_SIZE];
+extern const float ADA_SIMULATED_PRESSURE_SPEED[DATA_SIZE];
+// extern const float ADA_SIMULATED_NOISY_PRESSURE[DATA_SIZE];
\ No newline at end of file
diff --git a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
index aa7b9593174f4b0188ee70426df3347c8fdc9fb9..559f1b7598d22eca383300ad898d3ece454bbfc4 100644
--- a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
+++ b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp
@@ -1,6 +1,6 @@
/*
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Luca Mozzarelli
+ * Copyright (c) 2019-2021 Skyward Experimental Rocketry
+ * Authors: Luca Mozzarelli, Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -21,150 +21,241 @@
* THE SOFTWARE.
*/
-#define private public
-#define protected public
-
#ifdef STANDALONE_CATCH1_TEST
#include "catch/catch-tests-entry.cpp"
#endif
+#define EIGEN_NO_MALLOC
+
#include <Common.h>
-#include <ADA/ADAController.h>
-#include <events/Events.h>
#include <events/EventBroker.h>
+#include <events/Events.h>
#include <events/FSM.h>
#include <events/utils/EventCounter.h>
+
+#define private public
+#define protected public
+
+#include <ADA/ADA.h>
+#include <ADA/ADAController.h>
+
#include <algorithm>
#include <cmath>
#include <iostream>
#include <random>
#include <sstream>
#include <utils/testutils/catch.hpp>
+
#include "test-ada-data.h"
using namespace DeathStackBoard;
+using namespace ADAConfigs;
constexpr float NOISE_STD_DEV = 5; // Noise varaince
constexpr float LSB = 28;
constexpr unsigned int SHADOW_MODE_END_INDEX = 30;
-ADAController *ada_controller;
-unsigned seed = 1234567; // Seed for noise generation
+constexpr unsigned int APOGEE_SAMPLE = 382;
-float addNoise(float sample); // Function to add noise
-float quantization(float sample);
-void checkState(unsigned int i, KalmanState state);
+// Mock sensors for testing purposes
+class MockPressureSensor : public Sensor<PressureData>
+{
+public:
+ MockPressureSensor() {}
-std::default_random_engine generator(seed); // Noise generator
-std::normal_distribution<float> distribution(
- 0.0, NOISE_STD_DEV); // Noise generator distribution
+ bool init() override { return true; }
-typedef miosix::Gpio<GPIOG_BASE, 13> greenLed;
+ bool selfTest() override { return true; }
-TEST_CASE("Testing ada_controller from calibration to first descent phase")
+ PressureData sampleImpl() override
+ {
+ float press = 0.0;
+
+ if (before_liftoff)
+ {
+ press = addNoise(ADA_SIMULATED_PRESSURE[0]);
+ }
+ else
+ {
+ if (i < DATA_SIZE)
+ {
+ press = addNoise(ADA_SIMULATED_PRESSURE[i++]);
+ }
+ else
+ {
+ press = addNoise(ADA_SIMULATED_PRESSURE[DATA_SIZE - 1]);
+ }
+ }
+
+ return PressureData{TimestampTimer::getTimestamp(), press};
+ }
+
+ void signalLiftoff() { before_liftoff = false; }
+
+private:
+ volatile bool before_liftoff = true;
+ volatile unsigned int i = 0; // Last index
+ std::default_random_engine generator{1234567};
+ std::normal_distribution<float> distribution{0.0f, NOISE_STD_DEV};
+
+ float addNoise(float sample)
+ {
+ return quantization(sample + distribution(generator));
+ }
+
+ float quantization(float sample) { return round(sample / LSB) * LSB; }
+};
+
+class MockGPSSensor : public Sensor<GPSData>
+{
+public:
+ bool init() { return true; }
+ bool selfTest() { return true; }
+ GPSData sampleImpl() { return GPSData{}; }
+};
+
+MockPressureSensor mock_baro;
+MockGPSSensor mock_gps;
+
+using ADACtrl = ADAController<PressureData, GPSData>;
+ADACtrl *ada_controller;
+
+void checkState(unsigned int i, ADAKalmanState state)
{
- // Setting pin mode for signaling ada_controller status
+ if (i > 200)
{
- miosix::FastInterruptDisableLock dLock;
- greenLed::mode(miosix::Mode::OUTPUT);
+ if (state.x0 == Approx(ADA_SIMULATED_PRESSURE[i]).margin(70))
+ SUCCEED();
+ else
+ FAIL("i = " << i << "\t\t" << state.x0
+ << " != " << ADA_SIMULATED_PRESSURE[i]);
+
+ // Flying under the chutes the speed estimation is not very precise
+ if (i < 3000)
+ {
+ if (state.x1 == Approx(ADA_SIMULATED_PRESSURE_SPEED[i]).margin(80))
+ SUCCEED();
+ else
+ FAIL("i = " << i << "\t\t" << state.x1
+ << " != " << ADA_SIMULATED_PRESSURE_SPEED[i]);
+ }
}
+}
- ada_controller = new ADAController();
+TEST_CASE("Testing ada_controller from calibration to first descent phase")
+{
+ TimestampTimer::enableTimestampTimer();
+
+ ada_controller = new ADACtrl(mock_baro, mock_gps);
+ TRACE("ada init : %d \n", ada_controller->start());
// Start event broker and ada_controller
sEventBroker->start();
- ada_controller->start();
EventCounter counter{*sEventBroker};
counter.subscribe(TOPIC_ADA);
// Startup: we should be in idle
Thread::sleep(100);
- REQUIRE(ada_controller->testState(&ADAController::stateIdle));
+ REQUIRE(ada_controller->testState(&ADACtrl::state_idle));
// Enter Calibrating and REQUIRE
- sEventBroker->post({EV_CALIBRATE_ADA}, TOPIC_TC);
+ sEventBroker->post({EV_CALIBRATE}, TOPIC_FLIGHT_EVENTS);
Thread::sleep(100);
- REQUIRE(ada_controller->testState(&ADAController::stateCalibrating));
+ REQUIRE(ada_controller->testState(&ADACtrl::state_calibrating));
// Send baro calibration samples
for (unsigned i = 0; i < CALIBRATION_BARO_N_SAMPLES + 5; i++)
{
- ada_controller->updateBaro(addNoise(SIMULATED_PRESSURE[0]));
+ mock_baro.sample();
+ Thread::sleep(10);
+ ada_controller->update();
+
+ // TRACE("%d \n", i);
}
- // float mean = ada_controller->calibrator
- // if (mean == Approx(SIMULATED_PRESSURE[0]))
- // FAIL("Calibration value");
- // else
- // SUCCEED();
+ float mean = ada_controller->calibrator.getReferenceValues().ref_pressure;
+ if (mean == Approx(ADA_SIMULATED_PRESSURE[0]))
+ FAIL("Calibration value");
+ else
+ SUCCEED();
// Should still be in calibrating
Thread::sleep(100);
- REQUIRE(ada_controller->testState(&ADAController::stateCalibrating));
+ REQUIRE(ada_controller->testState(&ADACtrl::state_calibrating));
// Send set deployment altitude
ada_controller->setDeploymentAltitude(100);
- ada_controller->updateBaro(addNoise(SIMULATED_PRESSURE[0]));
+ mock_baro.sample();
+ Thread::sleep(10);
+ ada_controller->update();
// Should still be in calibrating
Thread::sleep(100);
- REQUIRE(ada_controller->testState(&ADAController::stateCalibrating));
+ REQUIRE(ada_controller->testState(&ADACtrl::state_calibrating));
// Send set altitude ref
ada_controller->setReferenceAltitude(1300);
- ada_controller->updateBaro(addNoise(SIMULATED_PRESSURE[0]));
+ mock_baro.sample();
+ Thread::sleep(10);
+ ada_controller->update();
// Should still be in calibrating
Thread::sleep(100);
- REQUIRE(ada_controller->testState(&ADAController::stateCalibrating));
+ REQUIRE(ada_controller->testState(&ADACtrl::state_calibrating));
// Send set temperature ref
ada_controller->setReferenceTemperature(15);
- ada_controller->updateBaro(addNoise(SIMULATED_PRESSURE[0]));
+ mock_baro.sample();
+ Thread::sleep(10);
+ ada_controller->update();
// Now we should be in ready
Thread::sleep(100);
- ada_controller->updateBaro(addNoise(SIMULATED_PRESSURE[0]));
+ mock_baro.sample();
+ Thread::sleep(10);
+ ada_controller->update();
Thread::sleep(100);
- REQUIRE(ada_controller->testState(&ADAController::stateReady));
-
- sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS);
+ REQUIRE(ada_controller->testState(&ADACtrl::state_ready));
// Send liftoff event: should be in shadow mode
+ sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS);
+ mock_baro.signalLiftoff();
Thread::sleep(100);
- REQUIRE(ada_controller->testState(&ADAController::stateShadowMode));
+ REQUIRE(ada_controller->testState(&ADACtrl::state_shadowMode));
long long shadow_mode_start = miosix::getTick();
- // Perform some checks while in shadow mode (to avoid triggering a false
- // apogee)
+ // Perform some chcmathecks while in shadow mode (to avoid triggering a
+ // false apogee)
for (unsigned i = 0; i < SHADOW_MODE_END_INDEX; i++)
{
- greenLed::high();
- float noisy_p = addNoise(SIMULATED_PRESSURE[i]);
- ada_controller->updateBaro(noisy_p);
+ // float noisy_p = addNoise(ADA_SIMULATED_PRESSURE[i]);
+ mock_baro.sample();
+ Thread::sleep(5);
+ ada_controller->update();
+ float noisy_p = mock_baro.getLastSample().press;
// Thread::sleep(100);
- KalmanState state = ada_controller->ada.getKalmanState();
+ ADAKalmanState state = ada_controller->ada.getKalmanState();
printf("%d,%f,%f,%f\n", (int)i, noisy_p, state.x0,
ada_controller->ada.getVerticalSpeed());
checkState(i, state);
- greenLed::low();
}
+
// Wait timeout
Thread::sleepUntil(shadow_mode_start + TIMEOUT_ADA_SHADOW_MODE);
// Should be active now
- REQUIRE(ada_controller->testState(&ADAController::stateActive));
+ REQUIRE(ada_controller->testState(&ADACtrl::state_active));
Thread::sleep(100);
// Send samples
-
- printf("%d\n", DATA_SIZE);
bool apogee_checked = false;
for (unsigned i = SHADOW_MODE_END_INDEX; i < DATA_SIZE; i++)
{
- greenLed::high();
- float noisy_p = addNoise(SIMULATED_PRESSURE[i]);
- ada_controller->updateBaro(noisy_p);
+ // float noisy_p = addNoise(ADA_SIMULATED_PRESSURE[i]);
+ mock_baro.sample();
+ Thread::sleep(5);
+ ada_controller->update();
+ float noisy_p = mock_baro.getLastSample().press;
// Thread::sleep(100);
- KalmanState state = ada_controller->ada.getKalmanState();
+ ADAKalmanState state = ada_controller->ada.getKalmanState();
printf("%d,%f,%f,%f\n", (int)i, noisy_p, state.x0,
ada_controller->ada.getVerticalSpeed());
checkState(i, state);
@@ -173,51 +264,21 @@ TEST_CASE("Testing ada_controller from calibration to first descent phase")
!apogee_checked)
{
apogee_checked = true;
- if (abs(i - 382) > 10)
+ if (fabs(i - APOGEE_SAMPLE) > 10)
{
- FAIL("Apogee error: " << (int)i - 382 << " samples");
+ FAIL("Apogee error: " << (int)i - APOGEE_SAMPLE << " samples");
}
else
{
- printf("Apogee error: %d samples\n", (int)(i - 382));
+ printf("Apogee error: %d samples\n", (int)(i - APOGEE_SAMPLE));
SUCCEED();
}
- REQUIRE(ada_controller->testState(&ADAController::statePressureStabilization));
+ Thread::sleep(1000);
+ REQUIRE(ada_controller->testState(
+ &ADACtrl::state_pressureStabilization));
Thread::sleep(EV_TIMEOUT_PRESS_STABILIZATION + 1000);
- REQUIRE(ada_controller->testState(&ADAController::stateFirstDescentPhase));
+ REQUIRE(ada_controller->testState(&ADACtrl::state_drogueDescent));
}
-
- greenLed::low();
}
-}
-
-void checkState(unsigned int i, KalmanState state)
-{
- if (i > 200)
- {
- if (state.x0 == Approx(SIMULATED_PRESSURE[i]).margin(70))
- SUCCEED();
- else
- FAIL("i = " << i << "\t\t" << state.x0
- << " != " << SIMULATED_PRESSURE[i]);
-
- // Flying under the chutes the speed estimation is not very precise
- if (i < 3000)
- {
- if (state.x1 == Approx(SIMULATED_PRESSURE_SPEED[i]).margin(80))
- SUCCEED();
- else
- FAIL("i = " << i << "\t\t" << state.x1
- << " != " << SIMULATED_PRESSURE_SPEED[i]);
- }
- }
-}
-
-float addNoise(float sample)
-{
- float noise = distribution(generator);
- return quantization(sample + noise);
-}
-
-float quantization(float sample) { return round(sample / LSB) * LSB; }
\ No newline at end of file
+}
\ No newline at end of file
diff --git a/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp b/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp
index 263fab645cd84c04046bd90de1f40ac65f948151..6ecdd1b79c1bfc12b8344d8a29e65690112c37ce 100644
--- a/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp
+++ b/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp
@@ -5,15 +5,19 @@
#define private public
-#include <Common.h>
#include <ADA/ADA.h>
+#include <Common.h>
+#include <configs/ADAconfig.h>
+
#include <iostream>
#include <random>
#include <sstream>
#include <utils/testutils/catch.hpp>
+
#include "test-kalman-acc-data.h"
using namespace DeathStackBoard;
+using namespace ADAConfigs;
constexpr float NOISE_STD_DEV_P = 5; // Noise varaince
constexpr float LSB_P = 28;
@@ -34,7 +38,7 @@ float addNoise_p(float sample);
float quantization_p(float sample);
std::normal_distribution<float> distribution_p(0.0, NOISE_STD_DEV_P);
-typedef miosix::Gpio<GPIOG_BASE, 13> greenLed;
+typedef miosix::Gpio<GPIOA_BASE, 5> greenLed;
TEST_CASE("Testing Kalman with accelerometer")
{
@@ -43,9 +47,9 @@ TEST_CASE("Testing Kalman with accelerometer")
ref_values.ref_altitude = 0;
ref_values.msl_pressure = SIMULATED_PRESSURE[1];
- ada = new ADA(ref_values);
+ ada = new ADA(ref_values, getKalmanConfig(), getKalmanAccConfig());
- KalmanState state;
+ ADAKalmanState state;
unsigned int j = 0;
for (unsigned int i = 0; i < DATA_SIZE_AX; i++)
{
@@ -58,7 +62,7 @@ TEST_CASE("Testing Kalman with accelerometer")
ada->updateBaro(addNoise_p(SIMULATED_PRESSURE[j]));
j++;
- state = ada->getKalmanState();
+ state = ada->getADAKalmanState();
std::cout << state.x0_acc << ", " << state.x1_acc << ", "
<< state.x2_acc << ", " << SIMULATED_PRESSURE[i] << ", "
<< ada->last_acc_average << "\n";
diff --git a/src/tests/catch/fsm/test-ada.cpp b/src/tests/catch/fsm/test-ada.cpp
index c24270294af5124abce2386e37d7964e8ba9892c..e94d70f4210cbd61ccbba9a5f58ec61e072575bb 100644
--- a/src/tests/catch/fsm/test-ada.cpp
+++ b/src/tests/catch/fsm/test-ada.cpp
@@ -1,6 +1,6 @@
/*
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Benedetta Margrethe Cattani & Luca Mozzarelli
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -27,70 +27,142 @@
// We need access to the handleEvent(...) function in state machines in order to
// test them synchronously
-#define protected public
-#define private public
#include <miosix.h>
+
+
#include <utils/testutils/catch.hpp>
-#include <ADA/ADAController.h>
-#include <events/Events.h>
-#include <configs/ADA_config.h>
+#define private public
+#define protected public
+
+#include "SensorManager/Sensors/Test/MockGPS.h"
+#include "SensorManager/Sensors/Test/MockPressureSensor.h"
+#include "events/Events.h"
#include "utils/testutils/TestHelper.h"
+#include "ADA/ADAController.h"
using miosix::Thread;
using namespace DeathStackBoard;
-using namespace CanInterfaces;
-class ADATestFixture
+using ADACtrl = ADAController<PressureData, GPSData>;
+
+class ADAControllerFixture
{
public:
- ADATestFixture() { ada = new ADAController(); }
- ~ADATestFixture()
+ // This is called at the beginning of each test / section
+ ADAControllerFixture()
+ {
+ sEventBroker->start();
+ controller = new ADACtrl(mock_baro, mock_gps);
+ controller->start();
+ }
+
+ // This is called at the end of each test / section
+ ~ADAControllerFixture()
{
- sEventBroker->unsubscribe(ada);
+ controller->stop();
+ sEventBroker->unsubscribe(controller);
sEventBroker->clearDelayedEvents();
- delete ada;
+ delete controller;
}
protected:
- ADAController* ada;
+ MockPressureSensor mock_baro;
+ MockGPS mock_gps;
+
+ ADACtrl* controller;
};
-TEST_CASE_METHOD(ADATestFixture, "Testing all transitions")
+TEST_CASE() {}
+
+TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from idle")
{
- SECTION("Testing CALIBRATION transitions")
+ controller->transition(&ADACtrl::state_idle);
+
+ SECTION("EV_CALIBRATE -> CALIBRATING")
{
- REQUIRE(testFSMTransition(*ada, Event{EV_ADA_READY}, &ADAController::stateIdle));
+ REQUIRE(testFSMTransition(*controller, Event{EV_CALIBRATE},
+ &ADACtrl::state_calibrating));
}
+}
- SECTION("Testing IDLE transitions")
+TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from calibrating")
+{
+ controller->transition(&ADACtrl::state_calibrating);
+
+ SECTION("EV_CALIBRATE_ADA -> CALIBRATING")
{
- REQUIRE(testFSMTransition(*ada, Event{EV_ADA_READY}, &ADAController::stateIdle));
-
- SECTION("IDLE->CALIBRATION")
- {
- REQUIRE(testFSMTransition(*ada, Event{EV_TC_CALIBRATE_ADA},
- &ADAController::stateCalibrating));
- }
-
- SECTION("IDLE->SHADOW_MODE")
- {
- REQUIRE(testFSMTransition(*ada, Event{EV_LIFTOFF},
- &ADAController::stateShadowMode));
- }
+ REQUIRE(testFSMTransition(*controller, Event{EV_CALIBRATE_ADA},
+ &ADACtrl::state_calibrating));
}
- SECTION("Testing all the transition from SHADOW_MODE")
+ SECTION("EV_ADA_READY -> READY")
{
- REQUIRE(testFSMTransition(*ada, Event{EV_ADA_READY}, &ADAController::stateIdle));
- REQUIRE(
- testFSMTransition(*ada, Event{EV_LIFTOFF}, &ADAController::stateShadowMode));
- REQUIRE(testFSMTransition(*ada, Event{EV_TIMEOUT_SHADOW_MODE},
- &ADAController::stateActive));
- REQUIRE(testFSMTransition(*ada, Event{EV_APOGEE},
- &ADAController::stateFirstDescentPhase));
- REQUIRE(
- testFSMTransition(*ada, Event{EV_DPL_ALTITUDE}, &ADAController::stateEnd));
+ REQUIRE(testFSMTransition(*controller, Event{EV_ADA_READY},
+ &ADACtrl::state_ready));
}
+}
+
+TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from ready")
+{
+ controller->transition(&ADACtrl::state_ready);
+
+ SECTION("EV_LIFTOFF -> SHADOW_MODE")
+ {
+ REQUIRE(testFSMTransition(*controller, Event{EV_LIFTOFF},
+ &ADACtrl::state_shadowMode));
+ }
+}
+
+TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from shadow_mode")
+{
+ controller->transition(&ADACtrl::state_shadowMode);
+
+ SECTION("EV_TIMEOUT_SHADOW_MODE -> ACTIVE")
+ {
+ REQUIRE(testFSMTransition(*controller, Event{EV_TIMEOUT_SHADOW_MODE},
+ &ADACtrl::state_active));
+ }
+}
+
+TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from active")
+{
+ controller->transition(&ADACtrl::state_active);
+
+ SECTION("EV_ADA_APOGEE_DETECTED -> PRESSURE_STABILIZATION")
+ {
+ REQUIRE(testFSMTransition(*controller, Event{EV_ADA_APOGEE_DETECTED},
+ &ADACtrl::state_pressureStabilization));
+ }
+}
+
+TEST_CASE_METHOD(ADAControllerFixture,
+ "Testing transitions from pressure_stabilization")
+{
+ controller->transition(&ADACtrl::state_pressureStabilization);
+
+ SECTION("EV_TIMEOUT_PRESS_STABILIZATION -> DROGUE_DESCENT")
+ {
+ REQUIRE(testFSMTransition(*controller,
+ Event{EV_TIMEOUT_PRESS_STABILIZATION},
+ &ADACtrl::state_drogueDescent));
+ }
+}
+
+TEST_CASE_METHOD(ADAControllerFixture,
+ "Testing transitions from drogue_descent")
+{
+ controller->transition(&ADACtrl::state_drogueDescent);
+
+ SECTION("EV_ADA_DPL_ALT_DETECTED -> END")
+ {
+ REQUIRE(testFSMTransition(*controller, Event{EV_ADA_DPL_ALT_DETECTED},
+ &ADACtrl::state_end));
+ }
+}
+
+TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from end")
+{
+ controller->transition(&ADACtrl::state_end);
}
\ No newline at end of file
diff --git a/src/tests/catch/fsm/test-aerobrakes.cpp b/src/tests/catch/fsm/test-aerobrakes.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b43d21f08c51bc0db648b0a4c90d3a6e911816cf
--- /dev/null
+++ b/src/tests/catch/fsm/test-aerobrakes.cpp
@@ -0,0 +1,205 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifdef STANDALONE_CATCH1_TEST
+#include "catch/catch-tests-entry.cpp"
+#endif
+
+// We need access to the handleEvent(...) function in state machines in order to
+// test them synchronously
+#define protected public
+
+#include <miosix.h>
+
+#include <utils/testutils/catch.hpp>
+
+#include "AeroBrakesController/AeroBrakesController.h"
+#include "AeroBrakesController/AeroBrakesServo.h"
+#include "configs/AeroBrakesConfig.h"
+#include "events/Events.h"
+#include "utils/testutils/TestHelper.h"
+
+using miosix::Thread;
+using namespace DeathStackBoard;
+
+class InputClass
+{
+public:
+ float z;
+ float vz;
+ float vMod;
+ uint64_t timestamp;
+};
+
+template <typename T>
+class MockSensor : public Sensor<T>
+{
+private:
+ int ts = 0;
+
+protected:
+ T sampleImpl() override
+ {
+ T data;
+ return data;
+ }
+
+ bool init() override { return true; }
+
+ bool selfTest() override { return true; }
+};
+
+class AeroBrakesControllerFixture
+{
+public:
+ // This is called at the beginning of each test / section
+ AeroBrakesControllerFixture()
+ {
+ controller = new AeroBrakesController<InputClass>(sensor);
+ sEventBroker->start();
+ controller->start();
+ }
+
+ // This is called at the end of each test / section
+ ~AeroBrakesControllerFixture()
+ {
+ controller->stop();
+ sEventBroker->unsubscribe(controller);
+ sEventBroker->clearDelayedEvents();
+ delete controller;
+ }
+
+protected:
+ AeroBrakesController<InputClass>* controller;
+
+ MockSensor<InputClass> sensor;
+};
+
+TEST_CASE_METHOD(AeroBrakesControllerFixture,
+ "Testing transitions from initialization")
+{
+ controller->transition(
+ &AeroBrakesController<InputClass>::state_initialization);
+
+ REQUIRE(
+ controller->testState(&AeroBrakesController<InputClass>::state_idle));
+}
+
+TEST_CASE_METHOD(AeroBrakesControllerFixture, "Testing transitions from idle")
+{
+ controller->transition(&AeroBrakesController<InputClass>::state_idle);
+
+ SECTION("EV_LIFTOFF -> SHADOW_MODE")
+ {
+ REQUIRE(testFSMTransition(
+ *controller, {EV_LIFTOFF},
+ &AeroBrakesController<InputClass>::state_shadowMode));
+ }
+
+ SECTION("EV_WIGGLE_SERVO -> IDLE")
+ {
+ REQUIRE(
+ testFSMTransition(*controller, {EV_WIGGLE_SERVO},
+ &AeroBrakesController<InputClass>::state_idle));
+ }
+
+ SECTION("EV_RESET_SERVO -> IDLE")
+ {
+ REQUIRE(
+ testFSMTransition(*controller, {EV_RESET_SERVO},
+ &AeroBrakesController<InputClass>::state_idle));
+ }
+
+ SECTION("EV_TEST_ABK -> TEST_AEROBRAKES")
+ {
+ REQUIRE(testFSMTransition(
+ *controller, {EV_TEST_ABK},
+ &AeroBrakesController<InputClass>::state_testAerobrakes));
+ }
+}
+
+TEST_CASE_METHOD(AeroBrakesControllerFixture,
+ "Testing transitions from shadow_mode")
+{
+ controller->transition(
+ &AeroBrakesController<InputClass>::state_shadowMode);
+
+ SECTION("EV_SHADOW_MODE_TIMEOUT -> ENABLED")
+ {
+ REQUIRE(testFSMTransition(
+ *controller, {EV_SHADOW_MODE_TIMEOUT},
+ &AeroBrakesController<InputClass>::state_enabled));
+ }
+
+ SECTION("EV_DISABLE_ABK -> DISABLED")
+ {
+ REQUIRE(testFSMTransition(
+ *controller, {EV_DISABLE_ABK},
+ &AeroBrakesController<InputClass>::state_disabled));
+ }
+}
+
+TEST_CASE_METHOD(AeroBrakesControllerFixture,
+ "Testing transitions from enabled")
+{
+ controller->transition(&AeroBrakesController<InputClass>::state_enabled);
+
+ SECTION("EV_APOGEE -> END")
+ {
+ REQUIRE(
+ testFSMTransition(*controller, {EV_APOGEE},
+ &AeroBrakesController<InputClass>::state_end));
+ }
+
+ SECTION("EV_DISABLE_ABK -> DISABLED")
+ {
+ REQUIRE(testFSMTransition(
+ *controller, {EV_DISABLE_ABK},
+ &AeroBrakesController<InputClass>::state_disabled));
+ }
+}
+
+TEST_CASE_METHOD(AeroBrakesControllerFixture, "Testing transitions from end")
+{
+ controller->transition(&AeroBrakesController<InputClass>::state_end);
+}
+
+TEST_CASE_METHOD(AeroBrakesControllerFixture,
+ "Testing transitions from disabled")
+{
+ controller->transition(&AeroBrakesController<InputClass>::state_disabled);
+}
+
+TEST_CASE_METHOD(AeroBrakesControllerFixture,
+ "Testing transitions from test_aerobrakes")
+{
+ controller->transition(
+ &AeroBrakesController<InputClass>::state_testAerobrakes);
+
+ SECTION("EV_TEST_TIMEOUT -> IDLE")
+ {
+ REQUIRE(
+ testFSMTransition(*controller, {EV_TEST_TIMEOUT},
+ &AeroBrakesController<InputClass>::state_idle));
+ }
+}
\ No newline at end of file
diff --git a/src/tests/catch/fsm/test-deployment.cpp b/src/tests/catch/fsm/test-deployment.cpp
index 0aee5bae40cba022e07da28ef548bb835384e6fd..ee3f2648abe036ac836650f86ecfa907ac6e6449 100644
--- a/src/tests/catch/fsm/test-deployment.cpp
+++ b/src/tests/catch/fsm/test-deployment.cpp
@@ -1,6 +1,6 @@
/*
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Someone
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -29,422 +29,158 @@
// test them synchronously
#define protected public
+#include <drivers/servo/servo.h>
#include <miosix.h>
+
#include <utils/testutils/catch.hpp>
#include "DeploymentController/DeploymentController.h"
+#include "configs/DeploymentConfig.h"
#include "events/Events.h"
-#include "utils/PinObserver.h"
#include "utils/testutils/TestHelper.h"
using miosix::Thread;
using namespace DeathStackBoard;
-/**
- * @brief Ensure cleanup in every test using RAIII
- *
- */
class DeploymentControllerFixture
{
public:
// This is called at the beginning of each test / section
DeploymentControllerFixture()
{
- dpl = new DeploymentController();
+ controller = new DeploymentController(&primaryCutter, &backupCutter,
+ &ejection_servo);
sEventBroker->start();
- dpl->start();
+ controller->start();
}
// This is called at the end of each test / section
~DeploymentControllerFixture()
{
- dpl->stop();
- sEventBroker->unsubscribe(dpl);
+ controller->stop();
+ sEventBroker->unsubscribe(controller);
sEventBroker->clearDelayedEvents();
- delete dpl;
+ delete controller;
}
protected:
- DeploymentController* dpl;
+ DeploymentController* controller;
+
+ HBridge primaryCutter{PrimaryCutterEna::getPin(), CUTTER_TIM,
+ CUTTER_CHANNEL_PRIMARY, PRIMARY_CUTTER_PWM_FREQUENCY,
+ PRIMARY_CUTTER_PWM_DUTY_CYCLE};
+ HBridge backupCutter{BackupCutterEna::getPin(), CUTTER_TIM,
+ CUTTER_CHANNEL_BACKUP, BACKUP_CUTTER_PWM_FREQUENCY,
+ BACKUP_CUTTER_PWM_DUTY_CYCLE};
+ DeploymentServo ejection_servo;
};
-/**
- * TEST_CASE_METHOD(Foo, "...") can access all protected members of Foo. See the
- * catch framework reference on Github.
- */
-TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from IDLE")
+TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from idle")
{
- SECTION("IDLE -> Opening Nosecone")
+ controller->transition(&DeploymentController::state_idle);
+
+ SECTION("DPL_IDLE -> EV_RESET_SERVO")
{
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_OPEN},
- &DeploymentController::state_spinning));
+ REQUIRE(testFSMTransition(*controller, Event{EV_RESET_SERVO},
+ &DeploymentController::state_idle));
}
- SECTION("IDLE -> Cutting main")
+ SECTION("DPL_IDLE -> EV_WIGGLE_SERVO")
{
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_MAIN},
- &DeploymentController::state_cuttingMain));
+ REQUIRE(testFSMTransition(*controller, Event{EV_WIGGLE_SERVO},
+ &DeploymentController::state_idle));
}
- SECTION("IDLE -> Cutting drogue")
+ SECTION("DPL_IDLE -> EV_NC_OPEN")
{
REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_DROGUE},
- &DeploymentController::state_cuttingDrogue));
+ testFSMTransition(*controller, Event{EV_NC_OPEN},
+ &DeploymentController::state_noseconeEjection));
}
-}
-
-TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from CUTTING MAIN")
-{
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_MAIN},
- &DeploymentController::state_cuttingMain));
+ SECTION("DPL_IDLE -> EV_CUT_DROGUE")
+ {
+ REQUIRE(testFSMTransition(*controller, Event{EV_CUT_DROGUE},
+ &DeploymentController::state_cuttingPrimary));
+ }
- SECTION(" Cutting Main -> Idle")
+ SECTION("DPL_IDLE -> EV_TEST_CUT_PRIMARY")
{
REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_CUTTING},
- &DeploymentController::state_idle));
+ testFSMTransition(*controller, Event{EV_TEST_CUT_PRIMARY},
+ &DeploymentController::state_testCuttingPrimary));
}
- SECTION("Deferred event: EV_CUT_DROGUE")
+ SECTION("DPL_IDLE -> EV_TEST_CUT_BACKUP")
{
- // Send CUT_DROGUE: nothing should happen
REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_DROGUE},
- &DeploymentController::state_cuttingMain));
- // Send TIMEOUT_CUTTING: back in idle
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_CUTTING},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_DROGUE should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue ));
+ testFSMTransition(*controller, Event{EV_TEST_CUT_BACKUP},
+ &DeploymentController::state_testCuttingBackup));
}
}
-TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from CUTTING DROGUE")
+TEST_CASE_METHOD(DeploymentControllerFixture,
+ "Testing transitions from nosecone_ejection")
{
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_DROGUE},
- &DeploymentController::state_cuttingDrogue));
+ controller->transition(&DeploymentController::state_noseconeEjection);
- SECTION("Cutting Drogue -> Idle")
+ SECTION("DPL_NOSECONE_EJECTION -> EV_NC_DETACHED")
{
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_CUTTING},
- &DeploymentController::state_idle));
+ REQUIRE(testFSMTransition(*controller, Event{EV_NC_DETACHED},
+ &DeploymentController::state_idle));
}
- SECTION("Deferred event: EV_CUT_MAIN")
+
+ SECTION("DPL_NOSECONE_EJECTION -> EV_NC_OPEN_TIMEOUT")
{
- // Send CUT_MAIN: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_MAIN},
- &DeploymentController::state_cuttingDrogue));
- // Send TIMEOUT_CUTTING: back in idle
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_CUTTING},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_MAIN should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain ));
+ REQUIRE(testFSMTransition(*controller, Event{EV_NC_OPEN_TIMEOUT},
+ &DeploymentController::state_idle));
}
}
-TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from SPINNING")
+TEST_CASE_METHOD(DeploymentControllerFixture,
+ "Testing transitions from cutting_primary")
{
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_OPEN},
- &DeploymentController::state_spinning));
- SECTION("SPINNING -> Waiting Detachment")
- {
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME},
- &DeploymentController::state_awaitingDetachment));
- }
- SECTION("SPINNING -> Waiting Min Open Time")
- {
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_DETACHED},
- &DeploymentController::state_awaitingOpenTime));
- }
- SECTION("OPENING NOSECONE -> Idle")
- {
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN},
- &DeploymentController::state_idle));
- }
- SECTION("Deferred event: EV_CUT_MAIN (spinning->idle)")
- {
- // Send CUT_MAIN: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_MAIN},
- &DeploymentController::state_spinning));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_MAIN should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain ));
- }
- SECTION("Deferred event: EV_CUT_MAIN (spinning->waiting time->idle)")
- {
- // Send CUT_MAIN: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_MAIN},
- &DeploymentController::state_spinning));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_DETACHED},
- &DeploymentController::state_awaitingOpenTime));
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_MAIN should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain ));
- }
- SECTION("Deferred event: EV_CUT_MAIN (spinning->waiting detachement->idle)")
- {
- // Send CUT_MAIN: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_MAIN},
- &DeploymentController::state_spinning));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME},
- &DeploymentController::state_awaitingDetachment));
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_DETACHED},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_MAIN should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain ));
- }
- SECTION("Deferred event: EV_CUT_DROGUE")
- {
- // Send CUT_DROGUE: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_DROGUE},
- &DeploymentController::state_spinning));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_DROGUE should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue ));
- }
- SECTION("Deferred event: EV_CUT_DROGUE (spinning->waiting time->idle)")
- {
- // Send CUT_DROGUE: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_DROGUE},
- &DeploymentController::state_spinning));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_DETACHED},
- &DeploymentController::state_awaitingOpenTime));
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_MAIN should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue ));
- }
- SECTION("Deferred event: EV_CUT_DROGUE (spinning->waiting detachement->idle)")
+ controller->transition(&DeploymentController::state_cuttingPrimary);
+
+ SECTION("DPL_CUTTING_PRIMARY -> EV_CUTTING_TIMEOUT")
{
- // Send CUT_DROGUE: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_DROGUE},
- &DeploymentController::state_spinning));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME},
- &DeploymentController::state_awaitingDetachment));
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_DETACHED},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_MAIN should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue ));
+ REQUIRE(testFSMTransition(*controller, Event{EV_CUTTING_TIMEOUT},
+ &DeploymentController::state_cuttingBackup));
}
}
-TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from WAITING MIN OPEN TIME")
+TEST_CASE_METHOD(DeploymentControllerFixture,
+ "Testing transitions from cutting_backup")
{
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_OPEN},
- &DeploymentController::state_spinning));
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_DETACHED},
- &DeploymentController::state_awaitingOpenTime));
+ controller->transition(&DeploymentController::state_cuttingBackup);
- SECTION("WAITING MIN OPEN TIME -> Idle")
- {
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME},
- &DeploymentController::state_idle));
- }
- SECTION("OPENING NOSECONE -> Idle")
+ SECTION("DPL_CUTTING_BACKUP -> EV_CUTTING_TIMEOUT")
{
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN},
- &DeploymentController::state_idle));
- }
- SECTION("Deferred event: EV_CUT_MAIN (timeout)")
- {
- // Send CUT_MAIN: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_MAIN},
- &DeploymentController::state_awaitingOpenTime));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_MAIN should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain ));
- }
- SECTION("Deferred event: EV_CUT_MAIN (min_open_time)")
- {
- // Send CUT_MAIN: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_MAIN},
- &DeploymentController::state_awaitingOpenTime));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_MAIN should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain ));
- }
- SECTION("Deferred event: EV_CUT_DROGUE (timeout)")
- {
- // Send CUT_DROGUE: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_DROGUE},
- &DeploymentController::state_awaitingOpenTime));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_DROGUE should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue ));
- }
- SECTION("Deferred event: EV_CUT_DROGUE (min_open_time)")
- {
- // Send CUT_DROGUE: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_DROGUE},
- &DeploymentController::state_awaitingOpenTime));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_DROGUE should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue ));
+ REQUIRE(testFSMTransition(*controller, Event{EV_CUTTING_TIMEOUT},
+ &DeploymentController::state_idle));
}
}
-TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from WAITING DETACHEMENT")
+TEST_CASE_METHOD(DeploymentControllerFixture,
+ "Testing transitions from test_cutting_primary")
{
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_OPEN},
- &DeploymentController::state_spinning));
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_MOT_MIN_OPEN_TIME},
- &DeploymentController::state_awaitingDetachment));
+ controller->transition(&DeploymentController::state_testCuttingPrimary);
- SECTION("WAITING DETACHEMENT -> Idle")
+ SECTION("DPL_TEST_CUTTING_PRIMARY -> EV_CUTTING_TIMEOUT")
{
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_DETACHED},
- &DeploymentController::state_idle));
- }
- SECTION("OPENING NOSECONE -> Idle")
- {
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN},
- &DeploymentController::state_idle));
- }
- SECTION("Deferred event: EV_CUT_MAIN (timeout)")
- {
- // Send CUT_MAIN: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_MAIN},
- &DeploymentController::state_awaitingDetachment));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_MAIN should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain ));
- }
- SECTION("Deferred event: EV_CUT_DROGUE (timeout)")
- {
- // Send CUT_DROGUE: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_DROGUE},
- &DeploymentController::state_awaitingDetachment));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_TIMEOUT_MOT_OPEN},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_DROGUE should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue ));
+ REQUIRE(testFSMTransition(*controller, Event{EV_CUTTING_TIMEOUT},
+ &DeploymentController::state_idle));
}
- SECTION("Deferred event: EV_CUT_MAIN (nc_detached)")
- {
- // Send CUT_MAIN: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_MAIN},
- &DeploymentController::state_awaitingDetachment));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_DETACHED},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_MAIN should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingMain ));
- }
- SECTION("Deferred event: EV_CUT_DROGUE (nc_detached)")
+}
+
+TEST_CASE_METHOD(DeploymentControllerFixture,
+ "Testing transitions from test_cutting_backup")
+{
+ controller->transition(&DeploymentController::state_testCuttingBackup);
+
+ SECTION("DPL_TEST_CUTTING_BACKUP -> EV_CUTTING_TIMEOUT")
{
- // Send CUT_DROGUE: nothing should happen
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_CUT_DROGUE},
- &DeploymentController::state_awaitingDetachment));
- // Go back in IDLE
- REQUIRE(
- testHSMTransition(*dpl, Event{EV_NC_DETACHED},
- &DeploymentController::state_idle));
- // Wait a bit to allow EV_ENTRY handling
- Thread::sleep(10);
- // CUT_DROGUE should now be processed
- REQUIRE( dpl->testState(&DeploymentController::state_cuttingDrogue ));
+ REQUIRE(testFSMTransition(*controller, Event{EV_CUTTING_TIMEOUT},
+ &DeploymentController::state_idle));
}
-}
\ No newline at end of file
+}
diff --git a/src/tests/deployment/test-deployment-interactive.cpp b/src/tests/deployment/test-deployment-interactive.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eac983f012c5d8c8e5e1f0ef195e93d44ced7221
--- /dev/null
+++ b/src/tests/deployment/test-deployment-interactive.cpp
@@ -0,0 +1,498 @@
+/**
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta
+ *
+ * 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 <logger/Logger.h>
+#include <miosix.h>
+
+#include <cstdio>
+#include <iostream>
+#include <sstream>
+#include <string>
+
+#include "DeploymentController/DeploymentController.h"
+#include "DeploymentController/DeploymentServo.h"
+#include "configs/CutterConfig.h"
+#include "events/Events.h"
+#include "utils/testutils/TestHelper.h"
+
+using namespace DeathStackBoard;
+
+using namespace std;
+
+void cuttingSequence();
+void cutterContinuity();
+void noseconeEjection();
+void wiggleServo();
+void setServoFullyOpen();
+void setServoFullyClose();
+void resetServo();
+void manualServoControl();
+void setServoParameters();
+void setPrimaryCutterParameters();
+void setBackupCutterParameters();
+void resetServoParameters();
+void resetPrimaryCutterParameters();
+void resetBackupCutterParameters();
+
+void waitUserInput();
+
+float minPosition = DeploymentConfigs::SERVO_MIN_POS;
+float maxPosition = DeploymentConfigs::SERVO_MAX_POS;
+float resetPosition = DeploymentConfigs::SERVO_RESET_POS;
+
+unsigned int primaryCutterFrequency =
+ CutterConfig::PRIMARY_CUTTER_PWM_FREQUENCY;
+float primaryCutterDutyCycle = CutterConfig::PRIMARY_CUTTER_PWM_DUTY_CYCLE;
+unsigned int backupCutterFrequency = CutterConfig::BACKUP_CUTTER_PWM_FREQUENCY;
+float backupCutterDutyCycle = CutterConfig::BACKUP_CUTTER_PWM_DUTY_CYCLE;
+float cutterTestDutyCycle = primaryCutterDutyCycle / 100.0f;
+
+int main()
+{
+ sEventBroker->start();
+
+ string temp;
+ for (;;)
+ {
+ int choice;
+ cout << "\n\nWhat do you want to do?: \n";
+ cout << "1. Test cutting sequence\n";
+ cout << "2. Continuity cutter test (non destructive)\n";
+ cout << "3. Nosecone ejection\n";
+ cout << "4. Servo wiggle (self test)\n";
+ cout << "5. Servo fully open\n";
+ cout << "6. Servo fully close\n";
+ cout << "7. Servo reset\n";
+ cout << "8. Servo manual control\n";
+ cout << "9. Set servo parameters\n";
+ cout << "10. Set primary cutter parameters\n";
+ cout << "11. Set backup cutter parameters\n";
+ cout << "12. Reset servo parameters (default)\n";
+ cout << "13. Reset primary cutter parameters (default)\n";
+ cout << "14. Reset backup cutter parameters (default)\n";
+
+ getline(cin, temp);
+ stringstream(temp) >> choice;
+
+ switch (choice)
+ {
+ case 1:
+ cuttingSequence();
+ break;
+ case 2:
+ cutterContinuity();
+ break;
+ case 3:
+ noseconeEjection();
+ break;
+ case 4:
+ wiggleServo();
+ break;
+ case 5:
+ setServoFullyOpen();
+ break;
+ case 6:
+ setServoFullyClose();
+ break;
+ case 7:
+ resetServo();
+ break;
+ case 8:
+ manualServoControl();
+ break;
+ case 9:
+ setServoParameters();
+ break;
+ case 10:
+ setPrimaryCutterParameters();
+ break;
+ case 11:
+ setBackupCutterParameters();
+ break;
+ case 12:
+ resetServoParameters();
+ break;
+ case 13:
+ resetPrimaryCutterParameters();
+ break;
+ case 14:
+ resetBackupCutterParameters();
+ break;
+ default:
+ cout << "Invalid option\n";
+ break;
+ }
+ }
+}
+
+int waitEventTimeout = 10 * 1000;
+
+void cuttingSequence()
+{
+ cout << "\n\n** CUTTER SEQUENCE TEST **\n\n";
+
+ waitUserInput();
+
+ // DeploymentController initialization
+ HBridge primaryCutter{PrimaryCutterEna::getPin(), CUTTER_TIM,
+ CUTTER_CHANNEL_PRIMARY, primaryCutterFrequency,
+ primaryCutterDutyCycle};
+ HBridge backupCutter{BackupCutterEna::getPin(), CUTTER_TIM,
+ CUTTER_CHANNEL_BACKUP, backupCutterFrequency,
+ backupCutterDutyCycle};
+ DeploymentServo ejection_servo;
+
+ DeploymentController deploymentController{&primaryCutter, &backupCutter,
+ &ejection_servo};
+
+ deploymentController.start();
+
+ Thread::sleep(1000);
+ sEventBroker->post({EV_CUT_DROGUE}, TOPIC_DPL);
+
+ waitForEvent(EV_CUTTING_TIMEOUT, TOPIC_DPL, waitEventTimeout);
+
+ cout << "Primary cutter is done.\n";
+
+ waitForEvent(EV_CUTTING_TIMEOUT, TOPIC_DPL, waitEventTimeout);
+ cout << "Backup cutter is done.\n";
+
+ deploymentController.stop();
+
+ cout << "\n\tTest finished!\n\n";
+}
+
+void cutterContinuity()
+{
+ string temp;
+ int cutter;
+
+ cout << "\n\n** CONTINUITY CUTTER TEST **\n\n";
+ cout << "Non-destructive cutter test : duty cycle inserted is divided by 100\n\n";
+ do
+ {
+ cout << "Which cutter to test? (1-primary / 2-backup)\n";
+
+ getline(cin, temp);
+ stringstream(temp) >> cutter;
+ } while (cutter != 1 && cutter != 2);
+
+ waitUserInput();
+
+ // DeploymentController initialization
+ HBridge primaryCutter{PrimaryCutterEna::getPin(), CUTTER_TIM,
+ CUTTER_CHANNEL_PRIMARY, primaryCutterFrequency,
+ cutterTestDutyCycle};
+ HBridge backupCutter{BackupCutterEna::getPin(), CUTTER_TIM,
+ CUTTER_CHANNEL_BACKUP, backupCutterFrequency,
+ cutterTestDutyCycle};
+ DeploymentServo ejection_servo;
+ DeploymentController deploymentController{&primaryCutter, &backupCutter,
+ &ejection_servo};
+
+ deploymentController.start();
+
+ if (cutter == 1)
+ {
+ cout << "Activating primary cutter...\n";
+ sEventBroker->post({EV_TEST_CUT_PRIMARY}, TOPIC_DPL);
+ }
+ else
+ {
+ cout << "Activating backup cutter...\n";
+ sEventBroker->post({EV_TEST_CUT_BACKUP}, TOPIC_DPL);
+ }
+
+ waitForEvent(EV_CUTTING_TIMEOUT, TOPIC_DPL);
+
+ cout << "Cutter test is done.\n";
+
+ deploymentController.stop();
+
+ cout << "\n\tTest finished!\n\n";
+}
+
+void noseconeEjection()
+{
+ cout << "\n\n** NOSECONE EJECTION TEST **\n\n";
+
+ waitUserInput();
+
+ EventCounter counter{*sEventBroker};
+
+ HBridge primaryCutter{PrimaryCutterEna::getPin(), CUTTER_TIM,
+ CUTTER_CHANNEL_PRIMARY, primaryCutterFrequency,
+ primaryCutterDutyCycle};
+ HBridge backupCutter{BackupCutterEna::getPin(), CUTTER_TIM,
+ CUTTER_CHANNEL_BACKUP, backupCutterFrequency,
+ backupCutterDutyCycle};
+ DeploymentServo ejection_servo;
+ DeploymentController deploymentController{&primaryCutter, &backupCutter,
+ &ejection_servo};
+
+ counter.subscribe(TOPIC_DPL);
+ counter.start();
+
+ cout << "Nosecone ejection in\n";
+ for (int i = 3; i > 0; i--)
+ {
+ cout << i << "\n";
+ Thread::sleep(1000);
+ }
+
+ deploymentController.start();
+ sEventBroker->post({EV_NC_OPEN}, TOPIC_DPL);
+
+ for (;;)
+ {
+ if (counter.getCount(EV_NC_OPEN_TIMEOUT) == 1)
+ break;
+ if (counter.getCount(EV_NC_DETACHED) >= 1)
+ break;
+ Thread::sleep(10);
+ }
+
+ cout << "Nosecone ejection test is done.\n";
+
+ deploymentController.stop();
+ counter.stop();
+
+ cout << "\n\tTest finished!\n\n";
+}
+
+void wiggleServo()
+{
+ cout << "\n\n** WIGGLE SERVO **\n\n";
+
+ waitUserInput();
+
+ DeploymentServo servo{minPosition, maxPosition, resetPosition};
+ servo.enable();
+ servo.reset();
+
+ cout << "Wiggling ...\n";
+ servo.selfTest();
+ Thread::sleep(1000);
+ servo.disable();
+
+ cout << "\n\tTest finished!\n\n";
+}
+
+void setServoFullyOpen()
+{
+ cout << "\n\n** SERVO FULLY OPEN **\n\n";
+
+ waitUserInput();
+
+ DeploymentServo servo{minPosition, maxPosition, resetPosition};
+ servo.enable();
+ servo.setMaxPosition();
+ Thread::sleep(1000);
+ servo.disable();
+
+ cout << "\n\tTest finished!\n\n";
+}
+
+void setServoFullyClose()
+{
+ cout << "\n\n** SERVO FULLY CLOSE **\n\n";
+
+ waitUserInput();
+
+ DeploymentServo servo{minPosition, maxPosition, resetPosition};
+ servo.enable();
+ servo.setMinPosition();
+ Thread::sleep(1000);
+ servo.disable();
+
+ cout << "\n\tTest finished!\n\n";
+}
+
+void resetServo()
+{
+ cout << "\n\n** RESET SERVO **\n\n";
+
+ waitUserInput();
+
+ DeploymentServo servo{minPosition, maxPosition, resetPosition};
+ servo.enable();
+ servo.reset();
+ Thread::sleep(1000);
+ servo.disable();
+
+ cout << "\n\tTest finished!\n\n";
+}
+
+void manualServoControl()
+{
+ cout << "\n\n** MANUAL SERVO CONTROL **\n\n";
+
+ string temp;
+ float angle;
+
+ do
+ {
+ cout << "Write the servo postion in degrees:\n";
+ getline(cin, temp);
+ stringstream(temp) >> angle;
+ } while (angle < minPosition || angle > maxPosition);
+
+ DeploymentServo servo{minPosition, maxPosition, resetPosition};
+
+ servo.enable();
+ servo.set(angle);
+ Thread::sleep(1000);
+ servo.disable();
+
+ cout << "\n\tTest finished!\n\n";
+}
+
+void setServoParameters()
+{
+ cout << "\n\n** SET SERVO PARAMETERS **\n\n";
+
+ string temp;
+ do
+ {
+ cout << "Write the servo minimum postion (degrees):\n";
+ getline(cin, temp);
+ stringstream(temp) >> minPosition;
+ } while (minPosition < 0 || minPosition > 180.0f);
+
+ do
+ {
+ cout << "Write the servo maximum postion (degrees):\n";
+ getline(cin, temp);
+ stringstream(temp) >> maxPosition;
+ } while (maxPosition < 0 || maxPosition > 180.0f);
+
+ do
+ {
+ cout << "Write the servo reset postion (degrees):\n";
+ getline(cin, temp);
+ stringstream(temp) >> resetPosition;
+ } while (resetPosition < 0 || resetPosition > 180.0f);
+
+
+ cout << "Configured servo parameteres:\n";
+ cout << "\tminimum position: " << minPosition << "\n";
+ cout << "\tmaximum position: " << maxPosition << "\n";
+ cout << "\treset position: " << resetPosition << "\n";
+
+ // Reset servo
+ DeploymentServo servo{minPosition, maxPosition, resetPosition};
+ servo.enable();
+ servo.reset();
+ Thread::sleep(1000);
+ servo.disable();
+}
+
+void setPrimaryCutterParameters()
+{
+ cout << "\n\n** SET PRIMARY CUTTER PARAMETERS **\n\n";
+
+ string temp;
+
+ cout << "Write the primary cutter frequency in hertz:\n";
+ getline(cin, temp);
+ stringstream(temp) >> primaryCutterFrequency;
+
+ do
+ {
+ cout << "Write the primary cutter duty cycle in %:\n";
+ getline(cin, temp);
+ stringstream(temp) >> primaryCutterDutyCycle;
+ primaryCutterDutyCycle /= 100;
+ } while (primaryCutterDutyCycle < 0 || primaryCutterDutyCycle > 1.0F);
+
+ cout << "Configured primary cutter parameteres:\n";
+ cout << "\tfrequency: " << primaryCutterFrequency << "Hz\n";
+ cout << "\tduty cycle: " << primaryCutterDutyCycle * 100 << "%\n";
+}
+
+void setBackupCutterParameters()
+{
+ cout << "\n\n** SET BACKUP CUTTER PARAMETERS **\n\n";
+
+ string temp;
+
+ cout << "Write the backup cutter frequency in hertz:\n";
+ getline(cin, temp);
+ stringstream(temp) >> backupCutterFrequency;
+
+ do
+ {
+ cout << "Write the backup cutter duty cycle in %:\n";
+ getline(cin, temp);
+ stringstream(temp) >> backupCutterDutyCycle;
+ backupCutterDutyCycle /= 100;
+ } while (backupCutterDutyCycle < 0 || backupCutterDutyCycle > 1.0F);
+
+ cout << "Configured backup cutter parameteres:\n";
+ cout << "\tfrequency: " << backupCutterFrequency << "Hz\n";
+ cout << "\tduty cycle: " << backupCutterDutyCycle * 100 << "%\n";
+}
+
+void resetServoParameters()
+{
+ minPosition = DeploymentConfigs::SERVO_MIN_POS;
+ maxPosition = DeploymentConfigs::SERVO_MAX_POS;
+ resetPosition = DeploymentConfigs::SERVO_MIN_POS;
+
+ cout << "Configured servo parameteres (default):\n";
+ cout << "\tminimum position: " << minPosition << "\n";
+ cout << "\tmaximum position: " << maxPosition << "\n";
+ cout << "\treset position: " << resetPosition << "\n";
+
+}
+
+void resetPrimaryCutterParameters()
+{
+ primaryCutterFrequency = CutterConfig::PRIMARY_CUTTER_PWM_FREQUENCY;
+ primaryCutterDutyCycle = CutterConfig::PRIMARY_CUTTER_PWM_DUTY_CYCLE;
+
+ cout << "Configured primary cutter parameteres:\n";
+ cout << "\tfrequency: " << primaryCutterFrequency << "Hz\n";
+ cout << "\tduty cycle: " << primaryCutterDutyCycle * 100 << "%\n";
+}
+
+void resetBackupCutterParameters()
+{
+ backupCutterFrequency = CutterConfig::BACKUP_CUTTER_PWM_FREQUENCY;
+ backupCutterDutyCycle = CutterConfig::BACKUP_CUTTER_PWM_DUTY_CYCLE;
+
+ cout << "Configured backup cutter parameteres:\n";
+ cout << "\tfrequency: " << backupCutterFrequency << "Hz\n";
+ cout << "\tduty cycle: " << backupCutterDutyCycle * 100 << "%\n";
+}
+
+void waitUserInput()
+{
+ string temp;
+
+ do
+ {
+ cout << "Write 'start' to begin the test:\n";
+ getline(cin, temp);
+ } while (temp != "start");
+}
\ No newline at end of file
diff --git a/src/tests/drivers/test-cutter.cpp b/src/tests/drivers/test-cutter.cpp
index 6d172e3072646180245c03fcdc0f15a25fbdd8e1..92153bbd026ffd0f794e0412bd790586f263e383 100644
--- a/src/tests/drivers/test-cutter.cpp
+++ b/src/tests/drivers/test-cutter.cpp
@@ -21,75 +21,173 @@
* THE SOFTWARE.
*/
-#include <interfaces-impl/hwmapping.h>
+//#include <interfaces-impl/hwmapping.h>
#include <miosix.h>
+
#include <iostream>
#include <sstream>
+//#include "configs/CutterConfig.h"
+#include "drivers/adc/InternalADC/InternalADC.h"
+#include "drivers/hbridge/HBridge.h"
+#include "sensors/analog/current/CurrentSensor.h"
-#include "DeploymentController/ThermalCutter/Cutter.h"
-#include "SensorManager/Sensors/ADCWrapper.h"
+#include "Common.h"
using namespace std;
using namespace miosix;
-using namespace DeathStackBoard;
+// using namespace DeathStackBoard;
+
+/**
+ * PWM : PE6 (both primary and backup)
+ * ENA : PD11 (primary)
+ * PG2 (backup)
+ * Csense : PF6 (primary)
+ * PF8 (backup)
+ */
-static constexpr int CUT_TIME = 60 * 1000;
+bool PRINT = true;
+static constexpr int CUT_TIME = 10 * 1000; // ms
+static constexpr int CSENSE_FREQ = 50; // hz
long long measured_cut_time = 0;
-void wait()
+bool finished = false;
+
+InternalADC::Channel ADC_CHANNEL_PRIMARY = InternalADC::Channel::CH4;
+InternalADC::Channel ADC_CHANNEL_BACKUP = InternalADC::Channel::CH6;
+ADC_TypeDef& ADCx = *ADC3;
+InternalADC adc(ADCx);
+
+static const PWM::Timer CUTTER_TIM{
+ TIM9, &(RCC->APB2ENR), RCC_APB2ENR_TIM9EN,
+ TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)};
+
+static const PWMChannel CUTTER_CHANNEL_PRIMARY = PWMChannel::CH2;
+static const PWMChannel CUTTER_CHANNEL_BACKUP = PWMChannel::CH2;
+
+using pwm = Gpio<GPIOE_BASE, 6>;
+
+using ena_primary = Gpio<GPIOD_BASE, 11>;
+using csens_primary = Gpio<GPIOF_BASE, 6>;
+
+using ena_backup = Gpio<GPIOG_BASE, 2>;
+using csens_backup = Gpio<GPIOF_BASE, 8>;
+
+HBridge* cutter;
+
+void wait(void* arg)
{
- long long t0 = getTick();
- for (long long t = t0; t < t0 + CUT_TIME; t += 50)
+ UNUSED(arg);
+
+ long long t = getTick();
+ long long t0 = t;
+
+ while (t < t0 + CUT_TIME)
{
- if (inputs::btn_open::value() == 0)
+ Thread::sleep(50);
+
+ t = getTick();
+
+ if (PRINT)
{
- break;
+ printf("Elapsed time : %.2f \n", (t - t0) / 1000.0);
}
- Thread::sleep(50);
}
- measured_cut_time = getTick() - t0;
- // printf("Stopped!\n");
-}
-bool print = false;
+ measured_cut_time = t - t0;
+
+ Thread::sleep(100);
+
+ if (!finished)
+ {
+ finished = true;
+ cutter->disable();
+ printf("\nTimer elapsed... Cutter disabled \n");
+ printf("Press ENTER to exit \n");
+ }
+}
-void csense(void*)
+void csense(void* args)
{
- ADCWrapper adc;
- adc.getCurrentSensorPtr()->init();
+ unsigned int c = *(unsigned int*)args;
- for (;;)
+ std::function<ADCData()> get_voltage_function;
+
+ if (c == 1)
{
- adc.getCurrentSensorPtr()->onSimpleUpdate();
- uint16_t raw1 =
- adc.getCurrentSensorPtr()->getCurrentDataPtr()->raw_value_1;
- uint16_t raw2 =
- adc.getCurrentSensorPtr()->getCurrentDataPtr()->raw_value_2;
-
- float current1 =
- adc.getCurrentSensorPtr()->getCurrentDataPtr()->current_1;
- float current2 =
- adc.getCurrentSensorPtr()->getCurrentDataPtr()->current_2;
- if (print)
- {
- printf("%d,%d,%d,%f,%f\n", (int)miosix::getTick(), (int)raw1,
- (int)raw2, current1, current2);
- }
- Thread::sleep(100);
+ get_voltage_function = []() {
+ return adc.getVoltage(ADC_CHANNEL_PRIMARY);
+ };
+ }
+ else
+ {
+ get_voltage_function = []() {
+ return adc.getVoltage(ADC_CHANNEL_BACKUP);
+ };
+ }
+
+ std::function<float(float)> adc_to_current = [](float adc_in) {
+ return (adc_in - 107.0f) * 32.4f;
+ };
+
+ CurrentSensor current_sensor(get_voltage_function, adc_to_current);
+
+ if (!adc.init() || !adc.selfTest())
+ {
+ printf("ERROR : %d\n", adc.getLastError());
+ }
+
+ current_sensor.init();
+
+ while (!finished)
+ {
+ adc.sample();
+ miosix::Thread::sleep(1000 / CSENSE_FREQ);
+ current_sensor.sample();
+
+ CurrentSenseData current_data = current_sensor.getLastSample();
+ printf("Time: %llu - ADC channel: %u - V:%f - A:%f \n",
+ current_data.adc_timestamp, current_data.channel_id,
+ current_data.voltage, current_data.current);
}
}
+void init()
+{
+ // Enable ADC3 clock
+ {
+ miosix::FastInterruptDisableLock dLock;
+
+ RCC->APB2ENR |= RCC_APB2ENR_ADC3EN; // <- CHANGE THIS!
+
+ pwm::mode(Mode::ALTERNATE);
+ pwm::alternateFunction(3);
+
+ ena_primary::mode(Mode::OUTPUT);
+ ena_primary::low();
+ csens_primary::mode(Mode::INPUT_ANALOG);
+
+ ena_backup::mode(Mode::OUTPUT);
+ ena_backup::low();
+ csens_backup::mode(Mode::INPUT_ANALOG);
+ }
+
+ TimestampTimer::enableTimestampTimer();
+
+ adc.enableChannel(ADC_CHANNEL_PRIMARY);
+ adc.enableChannel(ADC_CHANNEL_BACKUP);
+}
+
int main()
{
- Thread::create(csense, 2048);
+ init();
for (;;)
{
- print = false;
- printf(
- "Info: To stop cutting (and to stop the cut timer) press the OPEN "
- "button\n");
- printf("What do you want to cut? (1 - primary / 2 - backup)\n");
+ finished = false;
+ measured_cut_time = 0;
+
+ printf("\nWhat do you want to cut? (1 - primary / 2 - backup)\n");
+
unsigned int c, freq = 0;
float duty = 0;
string temp;
@@ -100,51 +198,72 @@ int main()
printf("Choose 1 or 2\n");
continue;
}
- cout << "Insert frequency (Hz): \n";
+ printf("Insert frequency (1-30000 Hz): \n");
getline(cin, temp);
stringstream(temp) >> freq;
- cout << "Insert duty cycle(%): \n";
+ printf("Insert duty cycle (1-100): \n");
getline(cin, temp);
stringstream(temp) >> duty;
- printf("Cutting %d, freq: %d, duty: %f\n", c, freq, duty);
+ printf("Cutting %d, freq: %d, duty: %f\n\n", c, freq, duty);
- if (!(freq > 1 && freq <= 30000 && duty >= 0.0f && duty <= 100.0f))
+ if (!(freq >= 1 && freq <= 30000 && duty >= 0.0f && duty <= 100.0f))
{
- printf("Wrong inputs!\n");
+ printf("Wrong inputs!\n\n");
continue;
}
do
{
- cout << "READY!\nWrite 'yeet' to begin:\n";
+ printf("READY!\nWrite 'start' to begin:\n");
getline(cin, temp);
- } while (temp != "yeet");
+ } while (temp != "start");
- {
- print = true;
+ printf("\n---------- Press ENTER to stop ----------\n");
+ printf("The PWM will automatically be disabled after %d s\n\n",
+ CUT_TIME / 1000);
+
+ Thread::create(csense, 2048, MAIN_PRIORITY, &c);
- Cutter cutter{freq, duty / 100, CUTTER_TEST_PWM_DUTY_CYCLE};
+ {
+ GpioPin* ena_pin;
+ PWMChannel channel;
if (c == 1)
{
- cutter.enablePrimaryCutter();
- wait();
- cutter.disablePrimaryCutter();
+ ena_pin = new GpioPin(ena_primary::getPin());
+ channel = CUTTER_CHANNEL_PRIMARY;
}
- else if (c == 2)
+ else // if (c == 2)
{
- cutter.enableBackupCutter();
- wait();
- cutter.disableBackupCutter();
+ ena_pin = new GpioPin(ena_backup::getPin());
+ channel = CUTTER_CHANNEL_BACKUP;
}
+
+ cutter = new HBridge(*ena_pin, CUTTER_TIM, channel, freq,
+ duty / 100.0f, 50);
+
+ cutter->enable();
+ Thread::create(wait, 2048);
+
+ // wait for the user to press ENTER or the timer to elapse
+ while (!finished)
+ {
+ getline(cin, temp);
+ finished = true;
+ printf("Stopped... \n\n");
+ }
+ cutter->disable();
+
+ delete cutter;
}
- Thread::sleep(2000);
- print = false;
+
Thread::sleep(500);
- printf("Cut Time: %.2f s\n", (measured_cut_time) / 1000.0f);
- printf("Done!\n\n\n");
+
+ printf("Cut Time: %.2f s\n\n", measured_cut_time / 1000.0f);
+ printf("Done!\n\n");
+ printf("----------------------------------------------------\n");
}
return 0;
diff --git a/src/tests/drivers/test-mavlink.cpp b/src/tests/drivers/test-mavlink.cpp
index e4d7045036719998c4948809384bd7eaf3a6fa22..3a10a74963d99a980819000bb6fce2672e7f5d4f 100644
--- a/src/tests/drivers/test-mavlink.cpp
+++ b/src/tests/drivers/test-mavlink.cpp
@@ -41,7 +41,7 @@ using namespace DeathStackBoard;
using MavChannel = MavlinkDriver<256,10>;
-Xbee_t* device;
+Xbee::Xbee* device;
MavChannel* channel;
// Receive function: sends an ACK back
@@ -67,9 +67,10 @@ static void onReceive(MavChannel* channel, const mavlink_message_t& msg)
int main()
{
enableXbeeInterrupt();
- busSPI2::init();
- device = new Xbee_t();
+ SPIBus xbee_bus(SPI2);
+ device = new Xbee::Xbee(xbee_bus, XbeeCS::getPin(), XbeeATTN::getPin(),
+ XbeeRST::getPin());
channel = new MavChannel(device, &onReceive, 250);
device->start();
diff --git a/src/tests/drivers/test-power-board.cpp b/src/tests/drivers/test-power-board.cpp
index d54868c0a49c167de6f27d1cb8145291d76527d5..7d4ab097478bf629a08b443b11a8974e454f8b92 100644
--- a/src/tests/drivers/test-power-board.cpp
+++ b/src/tests/drivers/test-power-board.cpp
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2018 Skyward Experimental Rocketry
+ * Copyright (c) 2018-2021 Skyward Experimental Rocketry
* Authors: Luca Erbetta
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
@@ -31,40 +31,50 @@ using namespace miosix;
int main()
{
- using namespace actuators;
- tcPwm::mode(Mode::OUTPUT);
-
- thCut1::ena::mode(Mode::OUTPUT);
- thCut1::ena::low();
- thCut1::csens::mode(Mode::INPUT_ANALOG);
-
- thCut2::ena::mode(Mode::OUTPUT);
- thCut2::ena::low();
- thCut2::csens::mode(Mode::INPUT_ANALOG);
+ using namespace actuators::nosecone;
+ using namespace actuators::airbrakes;
char c;
+
while(true)
{
+ printf("1 - Nosecone servo PWM \n");
+ printf("2 - Thermal cutter PWM \n");
+ printf("3 - Thermal cutter 1 enable \n");
+ printf("4 - Thermal cutter 2 enable \n");
+ printf("5 - Aerobrakes servo PWM \n");
+ printf("6 - Turn-off everything \n");
+ printf(">> ");
+
scanf("%c", &c);
switch(c)
{
- case 'a':
+ case '1':
+ nc_servo_pwm::high();
+ printf("nosecone servo pwm\n");
+ break;
+ case '2':
+ th_cut_pwm::high();
+ printf("thermal cutter pwm\n");
+ break;
+ case '3':
thCut1::ena::high();
printf("thCut1 ena\n");
break;
- case 'b':
+ case '4':
thCut2::ena::high();
printf("thCut2 ena\n");
break;
- case 'c':
- tcPwm::high();
- printf("tcPwm\n");
+ case '5':
+ airbrakes_servo_pwm::high();
+ printf("aerobrakes servo pwm\n");
break;
- case 'd':
+ case '6':
+ nc_servo_pwm::high();
thCut1::ena::low();
thCut2::ena::low();
- tcPwm::low();
+ th_cut_pwm::low();
printf("Closing everything\n");
break;
default:
diff --git a/src/tests/ram_test/README.md b/src/tests/ram_test/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..c57f1871281a713372c4db180dfb9777346fd16c
--- /dev/null
+++ b/src/tests/ram_test/README.md
@@ -0,0 +1,13 @@
+# RAM Test
+
+This entrypoint is useful to test the external RAM of a board.
+
+It writes into the external RAM and then reads back the content to check its correctness and also the memory retention capability.
+
+### Usage
+
+* In `ramtest.cpp` :
+ - Set the `ramBase` value to the starting address of the external memory, as defined in the linker script (e.g. `0xd0000000`).
+ - Set the `ramSize` value to the size of the external memory in bytes, as defined in the linker script (e.g. for 8 MB : `8*1024*1024`).
+* `__ENABLE_XRAM` must be defined, in Miosix's `config/Makefile.inc` (in the section corresponding to your board) or in `sbs.conf` (in the `ramtest` entrypoint section).
+* In `miosix-kernel/miosix/config/Makefile.inc`, in the section of your board, select the correct linker script. _The correct linker is not the one that defines the external RAM_. **You must select (uncomment) the linker script for your board that ends with "rom"**. This is a must, because the linker script could place some executable sections (such as the `bss`) in the external ram : the `ramtest` will then crash when trying to write to those same addresses.
\ No newline at end of file
diff --git a/src/tests/ram_test/main.cpp b/src/tests/ram_test/ramtest.cpp
similarity index 89%
rename from src/tests/ram_test/main.cpp
rename to src/tests/ram_test/ramtest.cpp
index d8e0e98624d3764354654628e1a86fd80551b3c6..3fc14d065119fdaee35a1ed73925f5de86984b3e 100644
--- a/src/tests/ram_test/main.cpp
+++ b/src/tests/ram_test/ramtest.cpp
@@ -65,10 +65,11 @@ template<typename T> bool ramTest()
int main()
{
- printf("\nBEFORE YOU START:\n");
- printf("1. Have you compiled with the right linker script (e.g. 2m+256k_rom)?\n");
- printf("2. Have you set RamBase and RamSize correctly in this entrypoint?\n");
- printf("3. Have you enable the RAM (aka compile with -D__ENABLE_XRAM)?\n");
+ printf("\n------- BEFORE YOU START: ------- \n");
+ printf("1. Have you read the README.md contained in this same folder?\n");
+ printf("2. Have you compiled with the right linker script (e.g. 2m+256k_rom)?\n");
+ printf("3. Have you set RamBase and RamSize correctly in this entrypoint?\n");
+ printf("4. Have you enabled the RAM (aka compile with -D__ENABLE_XRAM)?\n");
printf("Press enter to start...");
getchar();
@@ -80,6 +81,6 @@ int main()
if(ramTest<unsigned short>()) return 1;
iprintf("Testing byte size transfers\n");
if(ramTest<unsigned char>()) return 1;
- iprintf("Ok\n");
+ iprintf("Ok\n\n");
}
}
\ No newline at end of file
diff --git a/src/tests/test-dpl.cpp b/src/tests/test-dpl.cpp
deleted file mode 100644
index b15817914ac24e678fda1b4df027392c9b8bb13a..0000000000000000000000000000000000000000
--- a/src/tests/test-dpl.cpp
+++ /dev/null
@@ -1,325 +0,0 @@
-/**
- * Copyright (c) 2019 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
- *
- * 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 <logger/Logger.h>
-#include <cstdio>
-#include <iostream>
-#include <sstream>
-#include <string>
-
-#include "DeploymentController/DeploymentController.h"
-#include "DeploymentController/ThermalCutter/Cutter.h"
-#include "SensorManager/Sensors/ADCWrapper.h"
-#include "configs/CutterConfig.h"
-#include "events/EventInjector.h"
-#include "events/Events.h"
-#include "utils/testutils/TestHelper.h"
-
-using namespace DeathStackBoard;
-
-using namespace std;
-
-void cuttingSequence();
-void cutterContinuity();
-void noseconeEjection();
-void manualEvent();
-
-float adcToCurrent(uint16_t adc_in) { return (adc_in - 107) / 32.4f; }
-
-int main()
-{
- sEventBroker->start();
-
- string temp;
- for (;;)
- {
- int choice;
- cout << "\n\nWhat do you want to do?: \n";
- cout << "1. Test cutting sequence\n";
- cout << "2. Non-destructive cutter test\n";
- cout << "3. Nosecone ejection\n";
- cout << "4. Manual event input\n";
-
- getline(cin, temp);
- stringstream(temp) >> choice;
-
- switch (choice)
- {
- case 1:
- cuttingSequence();
- break;
- case 2:
- cutterContinuity();
- break;
- case 3:
- noseconeEjection();
- break;
- case 4:
- manualEvent();
- break;
- default:
- break;
- }
- }
-}
-
-bool print = false;
-
-void csense(void*)
-{
- ADCWrapper adc;
- adc.getCurrentSensorPtr()->init();
-
- for (;;)
- {
- adc.getCurrentSensorPtr()->onSimpleUpdate();
- uint16_t raw1 =
- adc.getCurrentSensorPtr()->getCurrentDataPtr()->raw_value_1;
- uint16_t raw2 =
- adc.getCurrentSensorPtr()->getCurrentDataPtr()->raw_value_2;
-
- float current1 =
- adc.getCurrentSensorPtr()->getCurrentDataPtr()->current_1;
- float current2 =
- adc.getCurrentSensorPtr()->getCurrentDataPtr()->current_2;
- if (print)
- {
- printf("%d,%d,%d,%f,%f\n", (int)miosix::getTick(), (int)raw1,
- (int)raw2, current1, current2);
- }
- Thread::sleep(100);
- }
-}
-
-void cuttingSequence()
-{
- string temp;
- char yn;
-
- cout << "\n\n** CUTTER SEQUENCE TEST **\n\n";
- cout << "Use predefined cutter parameters? (y/n)\n";
- getline(cin, temp);
- stringstream(temp) >> yn;
- unsigned int freq = CUTTER_PWM_FREQUENCY;
- float duty = CUTTER_PWM_DUTY_CYCLE * 100;
-
- if (yn == 'n')
- {
- do
- {
- cout << "Insert frequency (Hz): \n";
- getline(cin, temp);
- stringstream(temp) >> freq;
-
- cout << "Insert duty cycle(%): \n";
- getline(cin, temp);
- stringstream(temp) >> duty;
- } while (freq < 100 || freq > 30000 || duty < 0 || duty > 100);
-
- printf("Using custom parameters: Freq: %d, Duty: %.2f\n", freq, duty);
- }
- else
- {
- printf("Using default parameters: Freq: %d, Duty: %.2f\n", freq, duty);
- }
-
- do
- {
- cout << "Write 'start' to begin:\n";
- getline(cin, temp);
- } while (temp != "start");
-
- {
- Cutter c{freq, duty / 100.0f, CUTTER_TEST_PWM_DUTY_CYCLE};
- Servo s{DeploymentConfigs::SERVO_TIMER};
-
- DeploymentController dpl{c, s};
- dpl.start();
- print = true;
- Thread::sleep(1000);
- cout << "Activating primary cutter...\n";
- sEventBroker->post({EV_CUT_DROGUE}, TOPIC_DEPLOYMENT);
-
- waitForEvent(EV_TIMEOUT_CUTTING, TOPIC_DEPLOYMENT);
- cout << "Primary cutter is done.\n";
- cout << "Activating secondary cutter\n";
-
- waitForEvent(EV_TIMEOUT_CUTTING, TOPIC_DEPLOYMENT);
- cout << "Backup cutter is done.\n";
- cout << "\n Test finished!\n\n";
- Thread::sleep(1000);
- print = false;
- }
-}
-
-void cutterContinuity()
-{
- string temp;
- int cutter;
- cout << "\n\n** NON-DESTRUCTIVE CUTTER TEST **\n\n";
- do
- {
- cout << "Which cutter to test? (1-primary / 2-backup)\n";
-
- string temp;
- getline(cin, temp);
- stringstream(temp) >> cutter;
- } while (cutter != 1 && cutter != 2);
-
- cout << "Use predefined cutter parameters? (y/n)\n";
-
- char yn;
- getline(cin, temp);
- stringstream(temp) >> yn;
- unsigned int freq = CUTTER_PWM_FREQUENCY;
- float duty = CUTTER_TEST_PWM_DUTY_CYCLE * 100;
-
- if (yn == 'n')
- {
- do
- {
- cout << "Insert frequency (Hz): \n";
- getline(cin, temp);
- stringstream(temp) >> freq;
-
- cout << "Insert test duty cycle(%): \n";
- getline(cin, temp);
- stringstream(temp) >> duty;
- } while (freq < 100 || freq > 30000 || duty < 0 || duty > 100);
-
- printf("Using custom parameters: Freq: %d, Test Duty: %.2f\n", freq,
- duty);
- }
- else
- {
- printf("Using default parameters: Freq: %d, Test Duty: %.2f\n", freq,
- duty);
- }
-
- do
- {
- cout << "Write 'start' to begin:\n";
- getline(cin, temp);
- } while (temp != "start");
-
- {
- Cutter c{freq, CUTTER_PWM_DUTY_CYCLE, duty};
- Servo s{DeploymentConfigs::SERVO_TIMER};
-
- DeploymentController dpl{c, s};
- dpl.start();
- Thread::sleep(1000);
- print = true;
- if (cutter == 1)
- {
- cout << "Activating primary cutter...\n";
- sEventBroker->post({EV_TEST_CUTTER_PRIMARY}, TOPIC_DEPLOYMENT);
- }
- else
- {
- cout << "Activating backup cutter...\n";
- sEventBroker->post({EV_TEST_CUTTER_BACKUP}, TOPIC_DEPLOYMENT);
- }
-
- waitForEvent(EV_TIMEOUT_CUTTING, TOPIC_DEPLOYMENT);
-
- cout << "Cutter test is done.\n";
- cout << "\n Test finished!\n\n";
-
- Thread::sleep(1000);
- print = false;
-
- dpl.stop();
- }
-}
-
-void noseconeEjection()
-{
- Servo s{DeploymentConfigs::SERVO_TIMER};
- s.setMinPulseWidth(800);
- s.setMaxPulseWidth(2200);
- s.enable(DeploymentConfigs::SERVO_CHANNEL);
-
- s.setPosition(DeploymentConfigs::SERVO_CHANNEL,
- DeploymentConfigs::SERVO_RESET_POS);
-
- s.start();
-
- cout << "\n\n** NOSECONE EJECTION TEST **\n\n";
- string temp;
- do
- {
- cout << "Write 'yeet' to yeet out the nosecone:\n";
- getline(cin, temp);
- } while (temp != "yeet");
-
- {
- EventCounter counter{*sEventBroker};
-
- Cutter c{CUTTER_PWM_FREQUENCY, CUTTER_PWM_DUTY_CYCLE,
- CUTTER_TEST_PWM_DUTY_CYCLE};
-
- DeploymentController dpl{c, s};
- dpl.start();
- counter.subscribe(TOPIC_DEPLOYMENT);
- counter.start();
-
- cout << "Activating primary cutter...\n";
- sEventBroker->post({EV_NC_OPEN}, TOPIC_DEPLOYMENT);
-
- for (;;)
- {
- if (counter.getCount(EV_TIMEOUT_NC_OPEN) ==
- DeploymentConfigs::MAX_EJECTION_ATTEMPTS)
- break;
- if (counter.getCount(EV_NC_DETACHED) >= 1)
- break;
- Thread::sleep(10);
- }
-
- cout << "Cutter test is done.\n";
- cout << "\n Test finished!\n\n";
-
- dpl.stop();
- counter.stop();
- }
-}
-
-void manualEvent()
-{
- cout << "\n\n** MANUAL MODE **\n\n";
- cout << "Hope you know what you are doing\n";
-
- Cutter c{CUTTER_PWM_FREQUENCY, CUTTER_PWM_DUTY_CYCLE,
- CUTTER_TEST_PWM_DUTY_CYCLE};
-
- Servo s{DeploymentConfigs::SERVO_TIMER};
- s.setMinPulseWidth(800);
- s.setMaxPulseWidth(2200);
-
- DeploymentController dpl{c, s};
- EventInjector ei;
-
- ei.start();
- dpl.start();
-}
diff --git a/src/tests/test-hse.cpp b/src/tests/test-hse.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..15e850965ac2ef7d28965766314e9ad0c977e677
--- /dev/null
+++ b/src/tests/test-hse.cpp
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Luca Conterio
+ *
+ * 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 <miosix.h>
+#include <stdio.h>
+
+int main()
+{
+ printf("\nClock configuration : 0x%x \n\n", (unsigned int)RCC->CR);
+
+ // check if HSE is enabled
+ if (RCC->CR & RCC_CR_HSEON)
+ {
+ printf("HSE is on ... ok \n");
+ }
+ else
+ {
+ printf("HSE is off ... error \n");
+ }
+
+ // check if HSE is ready
+ if (RCC->CR & RCC_CR_HSERDY)
+ {
+ printf("HSE is ready ... ok \n");
+ }
+ else
+ {
+ printf("HSE not ready ... error \n");
+ }
+
+ // check if HSE is bypassed
+ if (RCC->CR & RCC_CR_HSEBYP)
+ {
+ printf("HSE is bypassed ... error \n");
+ }
+ else
+ {
+ printf("HSE is not bypassed ... ok \n");
+ }
+
+ // check if Clock Security System is enabled
+ if (RCC->CR & RCC_CR_CSSON)
+ {
+ printf("CSS is on \n");
+ }
+ else
+ {
+ printf("CSS is off \n");
+ }
+
+ for (;;)
+ {
+ }
+}
\ No newline at end of file
diff --git a/src/boards/DeathStack/DeploymentController/ThermalCutter/CutterData.h b/src/tests/test-pinhandler.cpp
similarity index 71%
rename from src/boards/DeathStack/DeploymentController/ThermalCutter/CutterData.h
rename to src/tests/test-pinhandler.cpp
index 39f6d04990cc912dead538e894931a53fa8c66ee..07be108d564e3a71fed20b8e33db448e49d64b8b 100644
--- a/src/boards/DeathStack/DeploymentController/ThermalCutter/CutterData.h
+++ b/src/tests/test-pinhandler.cpp
@@ -1,6 +1,5 @@
-/*
- * Copyright (c) 2018 Skyward Experimental Rocketry
- * Authors: Luca Erbetta
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Authors: Luca Conterio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -21,25 +20,27 @@
* THE SOFTWARE.
*/
-#pragma once
+#include "Common.h"
+#include "PinHandler/PinHandler.h"
-#include <ostream>
-#include <string>
+using namespace DeathStackBoard;
-namespace DeathStackBoard
+int main()
{
-
-enum class CutterState : uint8_t
-{
- IDLE,
- CUTTING_PRIMARY,
- CUTTING_BACKUP,
- TESTING_PRIMARY,
- TESTING_BACKUP
-};
+ TimestampTimer::enableTimestampTimer();
-struct CutterStatus
-{
- CutterState state = CutterState::IDLE;
-};
-} // namespace DeathStackBoard
+ PinHandler pin_handler;
+
+ if (!pin_handler.start())
+ {
+ printf("Error starting pin handler");
+ return -1;
+ }
+
+ while (true)
+ {
+ Thread::sleep(1000);
+ }
+
+ return 0;
+}
\ No newline at end of file
diff --git a/src/tests/test-sensormanager.cpp b/src/tests/test-sensormanager.cpp
index 9704712aba8a48335ac5f8a04c265865a6308549..a1aa0dd11c3978595ffc78cb19d26f4533bbbb39 100644
--- a/src/tests/test-sensormanager.cpp
+++ b/src/tests/test-sensormanager.cpp
@@ -34,9 +34,8 @@ using namespace DeathStackBoard;
int main()
{
Stats s;
- ADAController ada;
- SensorManager mgr{&ada};
- // ada.start();
+ SensorManager mgr;
+
try
{
LoggerService::getInstance()->start();
@@ -46,13 +45,13 @@ int main()
printf("SDCARD MISSING\n");
for (;;)
{
- led1::high();
+ ledOn();
Thread::sleep(200);
- led1::low();
+ ledOff();
Thread::sleep(200);
}
}
- led1::high();
+ ledOn();
sEventBroker->start();
mgr.start();
@@ -79,9 +78,9 @@ int main()
for (;;)
{
- led1::high();
+ ledOn();
Thread::sleep(1000);
- led1::low();
+ ledOff();
Thread::sleep(1000);
}
}