Skip to content
This repository was archived by the owner on Jun 28, 2025. It is now read-only.

Commit 0282fdf

Browse files
Uzayrhuss353Uzayrhuss353
committed
resolve merge conflict in SystemManager.cpp
2 parents 856a6f2 + 4022461 commit 0282fdf

File tree

1 file changed

+207
-33
lines changed

1 file changed

+207
-33
lines changed

‎SystemManager/Src/SystemManager.cpp‎

Lines changed: 207 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -4,35 +4,148 @@
44
//
55

66
#include "SystemManager.hpp"
7-
#include "sbus_receiver.hpp"
8-
#include "sbus_defines.h"
9-
#include "rcreceiver_datatypes.h"
7+
8+
#include <iostream>
9+
10+
#include "FreeRTOS.h"
11+
#include "GroundStationCommunication.hpp"
12+
#include "TelemetryManager.hpp"
13+
#include "cmsis_os.h"
1014
#include "drivers_config.hpp"
1115
#include "independent_watchdog.h"
16+
#include "rcreceiver_datatypes.h"
17+
#include "sbus_defines.h"
18+
#include "sbus_receiver.hpp"
1219
#include "tim.h"
1320

1421
#define DATANEW_TIMEOUT 75
22+
extern "C" {
23+
#include "app_fatfs.h"
24+
#include "log_util.h"
25+
}
26+
27+
#define TIMEOUT_CYCLES 250000 // 25k = 1 sec fro testing 10/14/2023 => 250k = 10 sec
28+
#define TIMOUT_MS 10000 // 10 sec
29+
30+
// 0 - AM, 1 - TM, 2 - PM
31+
static TaskHandle_t taskHandles[3];
1532

1633
static uint32_t DisconnectionCount = 0;
1734
float prevthrottle;
1835
float prevyaw;
1936
float prevroll;
2037
float prevpitch;
2138

22-
SystemManager::SystemManager():
23-
rcController_(sbus_pointer),
24-
throttleMotorChannel_(&htim2, TIM_CHANNEL_1),
25-
yawMotorChannel_(&htim2, TIM_CHANNEL_2),
26-
rollMotorChannel_(&htim2, TIM_CHANNEL_3),
27-
pitchMotorChannel_(&htim2, TIM_CHANNEL_4),
28-
invertedRollMotorChannel_(&htim3, TIM_CHANNEL_1),
29-
watchdog_(&hiwdg)
30-
{}
39+
SystemManager::SystemManager()
40+
: rcController_(sbus_pointer),
41+
throttleMotorChannel_(&htim2, TIM_CHANNEL_1),
42+
yawMotorChannel_(&htim2, TIM_CHANNEL_2),
43+
rollMotorChannel_(&htim2, TIM_CHANNEL_3),
44+
pitchMotorChannel_(&htim2, TIM_CHANNEL_4),
45+
invertedRollMotorChannel_(&htim3, TIM_CHANNEL_1),
46+
watchdog_(TIMOUT_MS) {
47+
// VARIABLES FOR TELEMETRY MANAGER TO HAVE AS REFERENCES THEY OBV SHOULD BE PUT SOMEWHERE ELSE,
48+
// BUT I FEEL LIKE SM PM WOULD KNOW WHERE. MAYBE IN THE HPP FILE? IDK HOW YOU ARE PLANNING ON
49+
// GATHERING THE DATA. I JUST PUT THEM HERE FOR NOW
50+
51+
52+
this->telemetryManager = setupTM();
53+
this->telemetryManager->init();
54+
55+
// IDK WHERE SM PLANS TO DO THIS, BUT telemetryManager.update() NEEDS TO BE CALLED AT A SEMI
56+
// REGULAR INTERVAL AS IT DEALS WITH MESSAGE DECODING AND LOW PRIORITY DATA TRANSMISSION
57+
}
58+
59+
TelemetryManager* SystemManager::setupTM() {
3160

32-
SystemManager::~SystemManager() {}
61+
// Struct containing the state of the drone
62+
TMStateData stateData;
63+
64+
// values to be assigned to stateData
65+
// THIS IS JUST A PLACEHOLDER, THE MEMORY ADDRESSES NEED TO LIVE LONGER THAN THE SM CONSTRUCTOR
66+
int32_t alt = 0;
67+
int32_t lat = 0;
68+
int32_t lon = 0;
69+
int32_t relative_alt = 0;
70+
int16_t vx = 0;
71+
int16_t vy = 0;
72+
int16_t vz = 0;
73+
uint16_t hdg = 0;
74+
int32_t time_boot_ms = 0;
75+
float roll = 0;
76+
float pitch = 0;
77+
float yaw = 0;
78+
float rollspeed = 0;
79+
float pitchspeed = 0;
80+
float yawspeed = 0;
81+
82+
// use the memory address of the above, since stateData uses pointers
83+
stateData.set_alt(&alt);
84+
stateData.set_lat(&lat);
85+
stateData.set_lon(&lon);
86+
stateData.set_relative_alt(&relative_alt);
87+
stateData.set_vx(&vx);
88+
stateData.set_vy(&vy);
89+
stateData.set_vz(&vz);
90+
stateData.set_hdg(&hdg);
91+
stateData.set_time_boot_ms(&time_boot_ms);
92+
stateData.set_roll(&roll);
93+
stateData.set_pitch(&pitch);
94+
stateData.set_yaw(&yaw);
95+
stateData.set_rollspeed(&rollspeed);
96+
stateData.set_pitchspeed(&pitchspeed);
97+
stateData.set_yawspeed(&yawspeed);
98+
99+
MAV_STATE mavState = MAV_STATE::MAV_STATE_STANDBY;
100+
MAV_MODE_FLAG mavMode = MAV_MODE_FLAG::MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
101+
102+
// Creating parameters for the GroundStationCommunication that will be passed to
103+
// telemetryManager
104+
TMCircularBuffer DMAReceiveBuffer = *(new TMCircularBuffer(rfd900_circular_buffer));
105+
106+
// the buffer that stores non_routine/low_priority bytes (ex. Battery Voltage) to be sent to the
107+
// ground station.
108+
uint8_t *lowPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE];
109+
110+
// the buffer that stores routine/high_priority bytes (ex. heading, general state data) to be
111+
// sent to the ground station.
112+
uint8_t *highPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE];
113+
114+
GroundStationCommunication GSC = *(new GroundStationCommunication(
115+
DMAReceiveBuffer, lowPriorityTransmitBuffer, highPriorityTransmitBuffer, RFD900_BUF_SIZE));
116+
117+
// the buffer that stores the bytes received from the ground station.
118+
MavlinkTranslator MT;
119+
120+
121+
return new TelemetryManager(stateData, mavState, mavMode, GSC, MT);
122+
}
123+
124+
// wrapper functions are needed as FreeRTOS xTaskCreate function does not accept functions that have
125+
// "this" pointers
126+
void SystemManager::attitudeManagerTaskWrapper(void *pvParameters) {
127+
SystemManager *systemManagerInstance = static_cast<SystemManager *>(pvParameters);
128+
systemManagerInstance->attitudeManagerTask();
129+
}
130+
131+
void SystemManager::telemetryManagerTaskWrapper(void *pvParameters) {
132+
SystemManager *systemManagerInstance = static_cast<SystemManager *>(pvParameters);
133+
systemManagerInstance->telemetryManagerTask();
134+
}
135+
136+
void SystemManager::pathManagerTaskWrapper(void *pvParameters) {
137+
SystemManager *systemManagerInstance = static_cast<SystemManager *>(pvParameters);
138+
systemManagerInstance->pathManagerTask();
139+
}
140+
141+
void SystemManager::systemManagerTask() {
142+
TickType_t xNextWakeTime = xTaskGetTickCount();
143+
uint16_t frequency = 5;
144+
145+
for (;;) {
146+
printf("SM called\r\n");
147+
HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_7); // toggle led light for testing
33148

34-
void SystemManager::flyManually() {
35-
for(;;){
36149
this->rcInputs_ = rcController_->GetRCControl();
37150

38151
//Is_Data_new implementation for failsafe
@@ -47,28 +160,89 @@ void SystemManager::flyManually() {
47160
DisconnectionCount = 0; //if the data is new, then we reset to 0.
48161
}
49162

50-
watchdog_.refreshWatchdog(); // always hit the dog
163+
watchdog_.refreshWatchdog(); // always hit the dog
51164

52-
if(this->rcInputs_.arm >= (SBUS_MAX/2)) {
53-
this->throttleMotorChannel_.set(rcInputs_.throttle);
54-
this->yawMotorChannel_.set(rcInputs_.yaw);
55-
this->rollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
56-
this->pitchMotorChannel_.set(SBUS_MAX - rcInputs_.pitch);
57-
this->invertedRollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
165+
if (this->rcInputs_.arm >= (SBUS_MAX / 2)) {
166+
this->throttleMotorChannel_.set(rcInputs_.throttle);
167+
this->yawMotorChannel_.set(rcInputs_.yaw);
168+
this->rollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
169+
this->pitchMotorChannel_.set(SBUS_MAX - rcInputs_.pitch);
170+
this->invertedRollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
58171

59-
prevthrottle = rcInputs_.throttle;
60-
prevyaw = rcInputs_.yaw;
61-
prevroll = rcInputs_.roll;
62-
prevpitch = rcInputs_.pitch;
172+
prevthrottle = rcInputs_.throttle;
173+
prevyaw = rcInputs_.yaw;
174+
prevroll = rcInputs_.roll;
175+
prevpitch = rcInputs_.pitch;
63176

64-
65-
}
66-
else{
177+
} else {
67178
this->throttleMotorChannel_.set(0);
68-
this->yawMotorChannel_.set(SBUS_MAX/2);
69-
this->rollMotorChannel_.set(SBUS_MAX/2);
70-
this->pitchMotorChannel_.set(SBUS_MAX/2);
71-
this->invertedRollMotorChannel_.set(SBUS_MAX/2);
179+
this->yawMotorChannel_.set(SBUS_MAX / 2);
180+
this->rollMotorChannel_.set(SBUS_MAX / 2);
181+
this->pitchMotorChannel_.set(SBUS_MAX / 2);
182+
this->invertedRollMotorChannel_.set(SBUS_MAX / 2);
72183
}
184+
185+
vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
186+
}
187+
}
188+
189+
void SystemManager::attitudeManagerTask() {
190+
TickType_t xNextWakeTime = xTaskGetTickCount();
191+
uint16_t frequency = 5;
192+
193+
for (;;) {
194+
// call AM
195+
196+
printf("AM called\r\n");
197+
HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_7); // toggle led light for testing
198+
vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
199+
}
200+
}
201+
202+
void SystemManager::telemetryManagerTask() {
203+
TickType_t xNextWakeTime = xTaskGetTickCount();
204+
uint16_t frequency = 5;
205+
206+
for (;;) {
207+
// call TM
208+
209+
printf("TM called\r\n");
210+
HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_9); // toggle led light for testing
211+
vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
212+
}
213+
}
214+
215+
void SystemManager::pathManagerTask() {
216+
TickType_t xNextWakeTime = xTaskGetTickCount();
217+
uint16_t frequency = 5;
218+
219+
for (;;) {
220+
// call PM
221+
222+
printf("PM called\r\n");
223+
vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
73224
}
74225
}
226+
227+
void SystemManager::startSystemManager() {
228+
printf("Initializing Tasks\r\n");
229+
230+
// enabling SD card logging
231+
if (MX_FATFS_Init() != APP_OK) {
232+
Error_Handler();
233+
}
234+
logInit();
235+
236+
// BaseType_t xTaskCreate( TaskFunction_t pvTaskCode, const char * const pcName,
237+
// configSTACK_DEPTH_TYPE usStackDepth, void * pvParameters, UBaseType_t uxPriority,
238+
// TaskHandle_t * pxCreatedTask );
239+
// function's name description size of
240+
// stack to allocate parameters for task priority
241+
// handler
242+
xTaskCreate(attitudeManagerTaskWrapper, "AM TASK", 800U, this, osPriorityNormal, taskHandles);
243+
xTaskCreate(telemetryManagerTaskWrapper, "TM TASK", 800U, this, osPriorityNormal,
244+
taskHandles + 1);
245+
// xTaskCreate(pathManagerTaskWrapper, "PM TASK", 800U, this, osPriorityNormal, taskHandles +2);
246+
247+
systemManagerTask();
248+
}

0 commit comments

Comments
 (0)