setup AM thread and modify flyManually() to use AMInput setter

SystemManager/Inc/SystemManager.hpp

@@ -9,10 +9,12 @@
 #include "rcreceiver_datatypes.h"
 #include "independent_watchdog.h"
 #include "ZP_D_PWMChannel.hpp"
+#include "AM.hpp"
 
 #define SBUS_THRESHOLD 25
 #define SBUS_MIN 0
 #define SBUS_MAX 100
+#define AM_PERIOD_MS 5
 
 class SystemManager {
 public:
@@ -22,6 +24,7 @@ class SystemManager {
 
  /* Class Functions */
  void flyManually();
+ static void runAM(void* pvParameters);
 
 private:
  SBUSReceiver rcController_;
@@ -32,6 +35,8 @@ class SystemManager {
  PWMChannel invertedRollMotorChannel_;
  PWMChannel pitchMotorChannel_;
  IndependentWatchdog watchdog_;
+ AM::AttitudeManager am_instance_;
+ TaskHandle_t AM_handle_ = NULL;
 };
 
 

SystemManager/Src/SystemManager.cpp

@@ -10,6 +10,7 @@
 #include "drivers_config.hpp"
 #include "independent_watchdog.h"
 #include "tim.h"
+#include "CommonDataTypes.hpp"
 
 SystemManager::SystemManager():
  rcController_(&huart2),
@@ -18,28 +19,48 @@ SystemManager::SystemManager():
  rollMotorChannel_(&htim2, TIM_CHANNEL_3),
  pitchMotorChannel_(&htim2, TIM_CHANNEL_4),
  invertedRollMotorChannel_(&htim3, TIM_CHANNEL_1),
- watchdog_(&hiwdg)
-{}
+ watchdog_(&hiwdg),
+ am_instance_(nullptr) // TODO: default flight mode? 
+{
+ // TODO: define priority levels
+ xTaskCreate(runAM, "AM Thread", 400U, (void*)&am_instance_, (configMAX_PRIORITIES / 2), &AM_handle_);
+}
+
+SystemManager::~SystemManager() {
+ vTaskDelete(AM_handle_);
+}
 
-SystemManager::~SystemManager() {}
+void SystemManager::runAM(void* pvParameters) {
+ AM::AttitudeManager* am_instance = (AM::AttitudeManager *) pvParameters;
+ TickType_t xLastWakeTime;
+ xLastWakeTime = xTaskGetTickCount();
+ for(;;) {
+ am_instance->runControlLoopIteration(AM::AttitudeManager::getControlInputs());
+ vTaskDelayUntil(&xLastWakeTime, AM_PERIOD_MS);
+ }
+}
 
 void SystemManager::flyManually() {
  for(;;){
  this->rcInputs_ = rcController_.GetRCControl();
  if (this->rcInputs_.isDataNew) watchdog_.refreshWatchdog();
+
+ AM::AttitudeManagerInput am_input = {
+ .roll = 0,
+ .pitch = 0,
+ .yaw = 0,
+ .throttle = 0
+ };
 
- if(this->rcInputs_.arm >= (SBUS_MAX/2)) {
- this->throttleMotorChannel_.set(rcInputs_.throttle);
- this->yawMotorChannel_.set(rcInputs_.yaw);
- this->rollMotorChannel_.set(rcInputs_.roll);
- this->pitchMotorChannel_.set(rcInputs_.pitch);
+ if (this->rcInputs_.arm >= (SBUS_MAX/2)) {
+ am_input.roll = rcInputs_.roll;
+ am_input.pitch = rcInputs_.pitch;
+ am_input.yaw = rcInputs_.yaw;
+ am_input.throttle = rcInputs_.throttle;
+ AM::AttitudeManager::setControlInputs(am_input);
  this->invertedRollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
- }
- else{
- this->throttleMotorChannel_.set(0);
- this->yawMotorChannel_.set(0);
- this->rollMotorChannel_.set(0);
- this->pitchMotorChannel_.set(0);
+ } else {
+ AM::AttitudeManager::setControlInputs(am_input);
  this->invertedRollMotorChannel_.set(0);
  }
  }