Command Manager
send sensor data every 100 ms
send actuator data every 100 ms
create vTask
mission_control_initiate(task_priority)
_task(void *params)
mission_control_handle_command(cJSON *json)
Should be startup function
Things that need to always happen continuously
UAV State
STATE_IDLE STATE_MODULE_STARTUP STATE_MODULE_CORE STATE_MODULE_TEARDOWN
switch(UAV STATE)
STATE_IDLE
check if there's something in the mission queue
run current_mission->startup_fun
pop mission from queue
set current_mission
set UAV STATE -> STATE_MODULE_STARTUP
run current_mission->teardown_fun
set UAV_STATE -> STATE_MODULE_IDLE
STATE_MODULE_TEARDOWN
calibrate sensors
turn on all sensors
while (true)
STATE_MODULE_STARTUP
set UAV_STATE -> STATE_MODULE_TEARDOWN
set UAV STATE -> STATE_MODULE_CORE
STATE_MODULE_CORE
run current_mission->core_fun
if (current_mission->completed)
sleep(10)
mission queue
mission struct ![[Mission Control#mission_t]]
...
...
Static Variables ![[Mission Control#Static Variables]]
TCP Socket Server
WebSocket Server
Idle Mission Module
FreeRTOS
while (True)
xSemaphoreTake(edf_lock)
while (edf_needs_updated())
if (edf1_power != edf1_target_power)
actuator_control_interface_set_tvc_x(double _x)
actuator_control_interface_set_tvc_y(double _y)
actuator_control_interface_calibrate_tvc_x_min(uint8_t _deg)
actuator_control_interface_calibrate_tvc_x_max(uint8_t _deg)
else
edf1_power -= edf1_rate
if(edf1_power > edf1_target_power)
edf1_power = edf1_target_power
if(edf1_power < edf1_target_power)
edf1_power += edf1_rate
if (edf2_power != edf2_target_power)
if (edf3_power != edf3_target_power)
if (edf4_power != edf4_target_power)
Note: this method will simply return if any of the edf power(s) don't equal their target value
This is being abstracted to a method for code readability
if(edf1_power < edf1_target_power)
edf1_power = edf1_target_power
set_pwm
actuator_control_interface_set_aileron_l(double _l)
actuator_control_interface_set_aileron_r(double _r)
actuator_control_interface_calibrate_tvc_y_min(uint8_t _deg)
actuator_control_interface_calibrate_tvc_y_max(uint8_t _deg)
actuator_control_interface_calibrate_aileron_l_min(uint8_t _deg)
actuator_control_interface_calibrate_aileron_l_max(uint8_t _deg)
actuator_control_interface_calibrate_aileron_r_min(uint8_t _deg)
actuator_control_interface_calibrate_aileron_r_max(uint8_t _deg)
actuator_control_interface_soft_set_edf_1(uint8_t power, uint8_t rate)
actuator_control_interface_soft_set_edf_2(uint8_t power, uint8_t rate)
actuator_control_interface_soft_set_edf_3(uint8_t power, uint8_t rate)
actuator_control_interface_soft_set_edf_4(uint8_t power, uint8_t rate)
actuator_control_interface_set_edf_1(uint8_t power)
actuator_control_interface_set_edf_2(uint8_t power)
actuator_control_interface_set_edf_3(uint8_t power)
actuator_control_interface_set_edf_4(uint8_t power)
ARM-M0+
Landing Mission Module
BMP280 Driver
HMC5883L Driver
I2C Bus Initialization
Initialize
Accelorometer Get X
Gyroscope Get Z
Gyroscope Get X
Accelorometer Get Y
Accelorometer Get Z
Gyroscope Get Y
Set Altitude
Get Altitude
Altitude Activate
Altitude Deactivate
Get Heading (in degrees)
Get Distance (in mm)
Set Heading (in degrees)
Compass Activate
MPU6050 (accel / gyro)
Accelorometer Set X
BMP280 (Pressure)
Accelorometer Set Z
Gyroscope Set X
QMC5883L (compass)
HCSR04 (ultrasonic)
Accelorometer Set Y
Read sensor
Read sensor
Compass Deactivate
if(mpu is active)
Read sensor
if(bmp is active)
Read sensor
if(error) error_count++
if(error) error_count++
if(error) error_count++
if(error) error_count++
Accelorometer Activate
Accelorometer Deactivate
#define MAX_ERROR_COUNT
int error_count = 0
Ultrasonic Activate
Ultrasonic Deactivate
sensor manager task
Reboot Devices
if(qmc is active)
if(hcsr is active)
Gyroscope Set Z
Gyroscope Set Y
Gyroscope Deactivate
Gyroscope Activate
while(error_count < MAX_ERROR_COUNT)
while(true) // loop forever
NOTE The sensor manager task will make sure that the I2C bus is up and running and that all sensors are properly functioning.. It will then be used to constantly read from sensors (when told to)
MPU6050 Driver
Loiter Mission Module
Takeoff Mission Module
AT24C02 Driver
SSD1306 Driver
Servo Driver
ESC Driver
Command Handler
Mission Planner
Control Panel
WebSocket Client
Accelerometer X: 0.5553765 Y: 0.0127444 Z: -0.568222
Gyroscope X: 0.5553765 Y: 0.0127444 Z: -0.568222
Barometric Pressure altitude: 233.7555 ft
Ultrasonic distance: 205.25552 mm
Magnetometer heading: 45.557
Flight Status: Stage: Startup System Health: Operational
Peripheral Status: Accelerometer: Operational Gyroscope: Operational Barometric Pressure: Operational Ultrasonic: Faulty Magnetometer: Disconnected
Motor Status: Motor 1: Operational Motor 2: Operational Motor 3: Operational Motor 4: Operational
Servo Status: Servo X: Operational Servo Y: Operational Servo L: Disconnected Servo R: Disconnected
Motor 1 Power: 10%
Motor 3 Power: 11%
Motor 2 Power: 12%
Motor 4 Power: 10%
Servo X Angle: 90
Servo Y Angle: 95
Servo L Angle: N/A
Servo R Angle: N/A
command injection:
Logs: ... ... ... ... ... ... ... ... ... ... ... ... ... ... ... ...
Accelerometer Graph (X, Y, Z)
Altitude Graph