diff --git a/CMakeLists.txt b/CMakeLists.txt
index fcab40c68440ccb04995edd25960281e3b96f978..b26523f565db5861cace2bceb5ec196b6841cc55 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -36,7 +36,8 @@ add_executable(parafoilTest-entry src/entrypoints/parafoilTest-entry.cpp
src/boards/ParafoilTest/Main/Sensors.cpp
src/boards/ParafoilTest/Main/Radio.cpp
src/boards/ParafoilTest/Wing/WingServo.cpp
- src/boards/ParafoilTest/Wing/WingAlgorithm.cpp)
+ src/boards/ParafoilTest/Wing/WingAlgorithm.cpp
+ src/boards/ParafoilTest/Wing/WingController.cpp)
sbs_target(parafoilTest-entry stm32f429zi_stm32f4discovery)
target_include_directories(parafoilTest-entry PRIVATE src/boards/ParafoilTest
src/)
diff --git a/data/ParafoilTestDev/UMLWingControlDiagram.dia b/data/ParafoilTestDev/UMLWingControlDiagram.dia
new file mode 100644
index 0000000000000000000000000000000000000000..b0e6eef0408c9501493cff203294678c28eba077
Binary files /dev/null and b/data/ParafoilTestDev/UMLWingControlDiagram.dia differ
diff --git a/data/ParafoilTestDev/UMLWingControlDiagram.png b/data/ParafoilTestDev/UMLWingControlDiagram.png
new file mode 100644
index 0000000000000000000000000000000000000000..8b359ce213df23faa91d2edf019a88d27d35ad02
Binary files /dev/null and b/data/ParafoilTestDev/UMLWingControlDiagram.png differ
diff --git a/src/boards/ParafoilTest/ParafoilTest.h b/src/boards/ParafoilTest/ParafoilTest.h
index 457549b6e1065a1d5c545fe6801805dbe091c245..15922e2bc43ab1ec35ece0dfe97888c062cbd816 100644
--- a/src/boards/ParafoilTest/ParafoilTest.h
+++ b/src/boards/ParafoilTest/ParafoilTest.h
@@ -24,6 +24,7 @@
#include <miosix.h>
#include <ParafoilTestStatus.h>
+#include <Wing/WingController.h>
#include <Main/Sensors.h>
#include <Main/Radio.h>
#include <events/EventBroker.h>
@@ -53,6 +54,11 @@ namespace ParafoilTestDev
*/
Sensors* sensors;
+ /**
+ * @brief Wing algorithm controller
+ */
+ WingController* wingController;
+
/**
* @brief Radio that manages the interaction between
* the xbee module and mavlink
@@ -76,19 +82,22 @@ namespace ParafoilTestDev
status.setError(&ParafoilTestStatus::eventBroker);
}
+ //Start the wing controller
+ wingController -> start();
+
//Start the sensors sampling
- if(!sensors -> start())
+ /*if(!sensors -> start())
{
LOG_ERR(log, "Error starting sensors");
status.setError(&ParafoilTestStatus::sensors);
- }
+ }*/
//Start the radio
- if(!radio -> start())
+ /*if(!radio -> start())
{
LOG_ERR(log, "Error starting the radio");
status.setError(&ParafoilTestStatus::radio);
- }
+ }*/
//After all the initializations i log the status
logger -> log(status);
@@ -140,12 +149,15 @@ namespace ParafoilTestDev
//Create the task scheduler
scheduler = new TaskScheduler();
+ //Create the wing controller
+ wingController = new WingController(scheduler);
+
//Create the sensors
- SPIBusInterface *spiInterface1 = new SPIBus(SPI1);
- sensors = new Sensors(*spiInterface1, scheduler);
+ //SPIBusInterface *spiInterface1 = new SPIBus(SPI1);
+ //sensors = new Sensors(*spiInterface1, scheduler);
//Create a new radio
- radio = new Radio(*spiInterface1);
+ //radio = new Radio(*spiInterface1);
}
/**
diff --git a/src/boards/ParafoilTest/ParafoilTestStatus.h b/src/boards/ParafoilTest/ParafoilTestStatus.h
index 7875c948ba512ce552f0fc9e89b876daee16cec9..1ddf969e6b70649155c0322e29dfd8c2e0003a72 100644
--- a/src/boards/ParafoilTest/ParafoilTestStatus.h
+++ b/src/boards/ParafoilTest/ParafoilTestStatus.h
@@ -44,10 +44,10 @@ namespace ParafoilTestDev
uint8_t parafoil_test = OK;
//Specific errors
- uint8_t logger = OK;
- uint8_t eventBroker = OK;
- uint8_t sensors = OK;
- uint8_t radio = OK;
+ uint8_t logger = OK;
+ uint8_t eventBroker = OK;
+ uint8_t sensors = OK;
+ uint8_t radio = OK;
/**
* @brief Method to set a specific component in an error state
diff --git a/src/boards/ParafoilTest/Wing/WingAlgorithm.cpp b/src/boards/ParafoilTest/Wing/WingAlgorithm.cpp
index 5f88a6291984e47d622d3927f0dc08b52a916297..0afd4b5f8e68a61b4d1b8f8f3b689755d4646669 100644
--- a/src/boards/ParafoilTest/Wing/WingAlgorithm.cpp
+++ b/src/boards/ParafoilTest/Wing/WingAlgorithm.cpp
@@ -38,13 +38,15 @@ namespace ParafoilTestDev
/**
* PUBLIC METHODS
*/
- WingAlgorithm::WingAlgorithm(WingServo* servo1, WingServo* servo2, const char* filename)
+ WingAlgorithm::WingAlgorithm(ServoInterface* servo1, ServoInterface* servo2, const char* filename)
: parser(filename)
{
- this -> servo1 = servo1;
- this -> servo2 = servo2;
+ setServo(servo1, servo2);
}
+ WingAlgorithm::WingAlgorithm(const char* filename)
+ : parser(filename){}
+
bool WingAlgorithm::init()
{
//Returns a std::vector which contains
@@ -56,6 +58,37 @@ namespace ParafoilTestDev
return fileValid;
}
+ void WingAlgorithm::setServo(ServoInterface* servo1, ServoInterface* servo2)
+ {
+ if(servo1 != NULL)
+ {
+ this -> servo1 = servo1;
+ }
+ else
+ {
+ //In this case i create a standard servo from the wing config file
+ servo1 = new WingServo(WING_SERVO1_TIMER,
+ WING_SERVO1_PWM_CHANNEL,
+ WING_SERVO1_MIN_POSITION,
+ WING_SERVO1_MAX_POSITION,
+ WING_SERVO1_RESET_POSITION);
+ }
+
+ if(servo2 != NULL)
+ {
+ this -> servo2 = servo2;
+ }
+ else
+ {
+ //In this case i create a standard servo from the wing config file
+ servo2 = new WingServo(WING_SERVO2_TIMER,
+ WING_SERVO2_PWM_CHANNEL,
+ WING_SERVO2_MIN_POSITION,
+ WING_SERVO2_MAX_POSITION,
+ WING_SERVO2_RESET_POSITION);
+ }
+ }
+
void WingAlgorithm::addStep(WingAlgorithmData step)
{
//I do it if and only if the file is invalid, because
@@ -70,6 +103,7 @@ namespace ParafoilTestDev
void WingAlgorithm::begin()
{
running = true;
+ shouldReset = true;
//Set the current timestamp
timeStart = TimestampTimer::getInstance().getTimestamp();
}
@@ -86,11 +120,17 @@ namespace ParafoilTestDev
*/
void WingAlgorithm::step()
{
- //Variable to remember the what is the stab that has to be done
+ //Variable to remember what is the step that has to be done
static unsigned int stepIndex = 0;
uint64_t currentTimestamp = TimestampTimer::getInstance().getTimestamp();
-
-
+
+ if(shouldReset)
+ {
+ //If the algorithm has been stopped
+ //i want to start from the beginning
+ stepIndex = 0;
+ shouldReset = false;
+ }
if(stepIndex >= steps.size())
{
@@ -104,9 +144,16 @@ namespace ParafoilTestDev
if(currentTimestamp - timeStart >= steps[stepIndex].timestamp)
{
- //I need to execute the current step
- servo1->set(steps[stepIndex].servo1Angle);
- servo2->set(steps[stepIndex].servo1Angle);
+ //TODO log the action
+ //I need to execute the current step (if not null servos)
+ if(servo1 != NULL)
+ {
+ servo1->set(steps[stepIndex].servo1Angle);
+ }
+ if(servo2 != NULL)
+ {
+ servo2->set(steps[stepIndex].servo2Angle);
+ }
//finally increment the stepIndex
stepIndex++;
diff --git a/src/boards/ParafoilTest/Wing/WingAlgorithm.h b/src/boards/ParafoilTest/Wing/WingAlgorithm.h
index d4d345a3f9a3eac02aca86eea5d611900b5f6c56..2887948dea1af484eedfcd99c03f408713cfaa5d 100644
--- a/src/boards/ParafoilTest/Wing/WingAlgorithm.h
+++ b/src/boards/ParafoilTest/Wing/WingAlgorithm.h
@@ -63,7 +63,7 @@ namespace ParafoilTestDev
{
struct WingAlgorithmData
{
- uint64_t timestamp; //First timestamp is 0
+ uint64_t timestamp; //First timestamp is 0 (in microseconds)
float servo1Angle; //degrees
float servo2Angle; //degrees
};
@@ -79,7 +79,14 @@ namespace ParafoilTestDev
* @param servo2 The second servo
* @param filename The csv file where all the operations are stored
*/
- WingAlgorithm(WingServo* servo1, WingServo* servo2, const char* filename);
+ WingAlgorithm(ServoInterface* servo1, ServoInterface* servo2, const char* filename);
+
+ /**
+ * @brief Construct a new Wing Algorithm object
+ *
+ * @param filename The csv file where all the operations are stored
+ */
+ WingAlgorithm(const char* filename);
/**
* @brief This method parses the file and stores it into a std::vector
@@ -89,6 +96,13 @@ namespace ParafoilTestDev
*/
bool init() override;
+ /**
+ * @brief Set the Servos objects
+ * @param servo1 The first algorithm servo
+ * @param servo2 The second algorithm servo
+ */
+ void setServo(ServoInterface* servo1, ServoInterface* servo2);
+
/**
* @brief Adds manually the step in case of fast debug needs
*
@@ -113,14 +127,19 @@ namespace ParafoilTestDev
void step() override;
//Actuators
- WingServo* servo1;
- WingServo* servo2;
+ ServoInterface* servo1;
+ ServoInterface* servo2;
//Offset timestamp
uint64_t timeStart;
//Procedure
std::vector<WingAlgorithmData> steps;
//File parser
CSVParser<WingAlgorithmData> parser;
- bool fileValid = false;
+ bool fileValid = false;
+ //This boolean is used to understand when to reset
+ //the index where the algorithm has stopped.
+ //In case of end call, we want to be able to perform
+ //another time this algorithm starting from 0
+ bool shouldReset = false;
};
}
\ No newline at end of file
diff --git a/src/boards/ParafoilTest/Wing/WingConfig.h b/src/boards/ParafoilTest/Wing/WingConfig.h
index b4e5e7e70f8c4c42bb122eb57cfd00bd8c83283f..213f00bf3b189ae3298a33b3291dfbc18707fb8e 100644
--- a/src/boards/ParafoilTest/Wing/WingConfig.h
+++ b/src/boards/ParafoilTest/Wing/WingConfig.h
@@ -31,6 +31,16 @@ using namespace Boardcore;
*/
namespace ParafoilTestDev
{
+ /**
+ * ALGORITHM CONFIGURATION
+ */
+ static const uint32_t WING_UPDATE_PERIOD = 10; //milliseconds
+ static const uint8_t WING_CONTROLLER_ID = 100; //TODO define a correct ID
+
+ /**
+ * SERVOS CONFIGURATIONS
+ */
+
//16 bit, 45MHz, no DMA timer
static TIM_TypeDef* WING_SERVO1_TIMER = TIM13;
static TIM_TypeDef* WING_SERVO2_TIMER = TIM14;
@@ -40,7 +50,7 @@ namespace ParafoilTestDev
//Servo dipendent variables
static const unsigned int WING_SERVO_MIN_PULSE = 1000;
- static const unsigned int WING_SERVO_MAX_PULSE = 2000;
+ static const unsigned int WING_SERVO_MAX_PULSE = 2500;
static const unsigned int WING_SERVO_FREQUENCY = 50;
static const float WING_SERVO1_MAX_POSITION = 180; //degrees
diff --git a/src/boards/ParafoilTest/Wing/WingController.cpp b/src/boards/ParafoilTest/Wing/WingController.cpp
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..ecd4ea7b77bc09317e11754ce363b6031db17a70 100644
--- a/src/boards/ParafoilTest/Wing/WingController.cpp
+++ b/src/boards/ParafoilTest/Wing/WingController.cpp
@@ -0,0 +1,158 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Wing/WingConfig.h>
+#include <Wing/WingController.h>
+
+namespace ParafoilTestDev
+{
+ /**
+ * PUBLIC METHODS
+ */
+ WingController::WingController(TaskScheduler* scheduler)
+ {
+ //Assign the task scheduler
+ this -> scheduler = scheduler;
+
+ //Set the current running state
+ this -> running = false;
+
+ //Set the current selected algorithm to 0
+ this -> selectedAlgorithm = 0;
+
+ //Initialize the servos, enable them,
+ //register the task into the task scheduler
+ init();
+ }
+
+ WingController::~WingController()
+ {
+ //Delete the servos
+ delete servo1;
+ delete servo2;
+ }
+
+ void WingController::addAlgorithm(const char* filename)
+ {
+ //Create the algorithm
+ WingAlgorithm* algorithm = new WingAlgorithm(servo1, servo2, filename);
+
+ //Add the algorithm to the vector and init it
+ algorithms.push_back(algorithm);
+ //If init fails[Beacuse of inexistent file or stuff] doesn't matter
+ //Because the algorithm is empty and so it won't do anything
+ algorithms[algorithms.size() - 1] -> init();
+ }
+
+ void WingController::addAlgorithm(WingAlgorithm* algorithm)
+ {
+ //Ensure that the servos are correct
+ algorithm->setServo(servo1, servo2);
+
+ //Add the algorithm to the vector
+ algorithms.push_back(algorithm);
+ }
+
+ void WingController::selectAlgorithm(unsigned int index)
+ {
+ if(index >= 0 && index < algorithms.size())
+ {
+ selectedAlgorithm = index;
+ }
+ else
+ {
+ //I select the 0 algorithm
+ selectedAlgorithm = 0;
+ }
+ }
+
+ void WingController::start()
+ {
+ //If the selected algorithm is valid --> also the
+ //algorithms array is not empty i start the whole thing
+ if(selectedAlgorithm >= 0 && selectedAlgorithm < algorithms.size())
+ {
+ //Set the boolean that enables the update method to true
+ running = true;
+
+ //Begin the selected algorithm
+ algorithms[selectedAlgorithm] -> begin();
+
+ //Also start the scheduler
+ scheduler -> start();
+ }
+ }
+
+ void WingController::stop()
+ {
+ //Set running to false so that the update method doesn't act
+ running = false;
+ //Stop the algorithm if selected
+ if(selectedAlgorithm >= 0 && selectedAlgorithm < algorithms.size())
+ {
+ algorithms[selectedAlgorithm] -> end();
+ }
+ }
+
+ void WingController::update()
+ {
+ //Check if the algorithm is running
+ if(!running)
+ {
+ return;
+ }
+
+ //If the selected algorithm is valid i can update it
+ if(selectedAlgorithm >= 0 && selectedAlgorithm < algorithms.size())
+ {
+ algorithms[selectedAlgorithm] -> update();
+ }
+ }
+
+ /**
+ * PRIVATE METHODS
+ */
+ void WingController::init()
+ {
+ //Instanciate the servos
+ servo1 = new WingServo(WING_SERVO1_TIMER,
+ WING_SERVO1_PWM_CHANNEL,
+ WING_SERVO1_MIN_POSITION,
+ WING_SERVO1_MAX_POSITION,
+ WING_SERVO1_RESET_POSITION);
+
+ servo2 = new WingServo(WING_SERVO2_TIMER,
+ WING_SERVO2_PWM_CHANNEL,
+ WING_SERVO2_MIN_POSITION,
+ WING_SERVO2_MAX_POSITION,
+ WING_SERVO2_RESET_POSITION);
+
+ //Enable servos
+ servo1 -> enable();
+ servo2 -> enable();
+
+ //Register the task
+ TaskScheduler::function_t updateFunction([=]() {update();});
+
+ scheduler -> addTask(updateFunction, WING_UPDATE_PERIOD,
+ WING_CONTROLLER_ID);
+ }
+}
\ No newline at end of file
diff --git a/src/boards/ParafoilTest/Wing/WingController.h b/src/boards/ParafoilTest/Wing/WingController.h
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..c0eeace1bbceec0baa045a23871c6af160cec149 100644
--- a/src/boards/ParafoilTest/Wing/WingController.h
+++ b/src/boards/ParafoilTest/Wing/WingController.h
@@ -0,0 +1,143 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <Wing/WingAlgorithm.h>
+#include <scheduler/TaskScheduler.h>
+
+/**
+ * @brief This class allows the user to select the wing algorithm
+ * that has to be used during the tests. It also registers his
+ * dedicated function in the task schduler in order to be
+ * executed every fixed period and to update the two servos position
+ * depending on the selected algorithm.
+ *
+ * Use case example:
+ * controller = new WingController(scheduler);
+ *
+ * controller.addAlgorithm("filename");
+ * OR
+ * controller.addAlgorithm(algorithm);
+ *
+ * controller.selectAlgorithm(index);
+ *
+ * controller.start();
+ * controller.stop(); //If you want to abort the execution
+ * controller.start(); //If you want to start again from the beginning
+ */
+
+using namespace Boardcore;
+
+namespace ParafoilTestDev
+{
+ class WingController
+ {
+ private:
+ /**
+ * @brief Servo actuators
+ */
+ ServoInterface* servo1;
+ ServoInterface* servo2;
+
+ /**
+ * @brief List of loaded algorithms (from SD or not)
+ */
+ std::vector<WingAlgorithm*> algorithms;
+
+ /**
+ * @brief The common task scheduler
+ */
+ TaskScheduler* scheduler;
+
+ /**
+ * @brief This attribute is modified by the mavlink radio section.
+ * The user using the Ground Station can select the pre-enumered algorithm to execute
+ */
+ unsigned int selectedAlgorithm;
+
+ /**
+ * @brief Internal running state
+ */
+ bool running;
+
+ /**
+ * @brief Initialization method. It registers the update method
+ * into the task scheduler
+ */
+ void init();
+
+ public:
+
+ /**
+ * @brief Construct a new Wing Controller object
+ *
+ * @param scheduler The main used task scheduler
+ */
+ WingController(TaskScheduler* scheduler);
+
+ /**
+ * @brief Destroy the Wing Controller object.
+ * In particular destroys the servo instances
+ */
+ ~WingController();
+
+ /**
+ * @brief Method to add the algorithm in the list
+ *
+ * @param filename The SD file where to read the instructions
+ */
+ void addAlgorithm(const char* filename);
+
+ /**
+ * @brief Method to add the algorithm in the list
+ *
+ * @param algorithm The algorithm with
+ * all already done (e.g. steps already registered)
+ */
+ void addAlgorithm(WingAlgorithm* algorithm);
+
+ /**
+ * @brief Selects the algorithm if present.
+ *
+ * @param index The algorithms vector index
+ */
+ void selectAlgorithm(unsigned int index);
+
+ /**
+ * @brief Sets the internal state running and
+ * starts the selected algorithm
+ */
+ void start();
+
+ /**
+ * @brief Sets the internal state to stop and
+ * stops the selected algorithm
+ */
+ void stop();
+
+ /**
+ * @brief Method that is called every time period
+ * to update the internal wing servos states
+ */
+ void update();
+ };
+}
\ No newline at end of file
diff --git a/src/entrypoints/parafoilTest-entry.cpp b/src/entrypoints/parafoilTest-entry.cpp
index 91870acde730844c4fcd02505b08109bac08a66e..e808da3226d86e392529317ed9b11694b927bc26 100644
--- a/src/entrypoints/parafoilTest-entry.cpp
+++ b/src/entrypoints/parafoilTest-entry.cpp
@@ -19,14 +19,30 @@
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
+#include <miosix.h>
#include <ParafoilTest.h>
using namespace ParafoilTestDev;
int main()
{
+ miosix::GpioPin servo(GPIOA_BASE, 6);
+ servo.mode(miosix::Mode::ALTERNATE);
+ servo.alternateFunction(9);
+
+ miosix::GpioPin servo2(GPIOA_BASE, 7);
+ servo2.mode(miosix::Mode::ALTERNATE);
+ servo2.alternateFunction(9);
+
//TODO integrate all the logging stuff
ParafoilTest::getInstance().start();
+ miosix::Thread::sleep(2000);
+ ParafoilTest::getInstance().wingController->stop();
+ ParafoilTest::getInstance().wingController->start();
+
+ while(true)
+ {}
+
return 0;
}