dc2bc678e4
# Conflicts: # UAV-ControlSystem/inc/system_variables.h # UAV-ControlSystem/src/Flight/pid.c # UAV-ControlSystem/src/main.c # UAV-ControlSystem/src/tasks_main.c
301 lines
8.1 KiB
C
301 lines
8.1 KiB
C
/**************************************************************************
|
|
* NAME: tasks_main.c *
|
|
* *
|
|
* AUTHOR: Jonas Holmberg *
|
|
* *
|
|
* PURPOSE: Implement all the functions that will be called when *
|
|
* executing a task in the scheduler. *
|
|
* *
|
|
* INFORMATION: Holds the function implementations for the individual tasks*
|
|
* that are invoked when a task is executed in the scheduler. *
|
|
* Each task needs to have an associated function that has to *
|
|
* be invoked when it is chosen as the task to run. *
|
|
* Additionally optional event driven task functions must be *
|
|
* implemented here as well. This file will include different *
|
|
* drivers meaning that the task functions could jump around *
|
|
* into other files before finishing its execution. *
|
|
* *
|
|
* GLOBAL VARIABLES: *
|
|
* Variable Type Description *
|
|
* -------- ---- ----------- *
|
|
***************************************************************************/
|
|
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
|
|
#include "Scheduler/scheduler.h"
|
|
#include "Scheduler/tasks.h"
|
|
#include "stm32f4xx_revo.h"
|
|
|
|
/* Drivers */
|
|
#include "drivers/led.h"
|
|
#include "drivers/adc.h"
|
|
#include "drivers/motors.h"
|
|
#include "drivers/pwm.h"
|
|
#include "drivers/system_clock.h"
|
|
#include "config/eeprom.h"
|
|
#include "config/cli.h"
|
|
#include "drivers/sbus.h"
|
|
#include "drivers/failsafe_toggles.h"
|
|
#include "drivers/I2C.h"
|
|
#include "drivers/accel_gyro.h"
|
|
#include "drivers/motormix.h"
|
|
#include "Flight/pid.h"
|
|
#include "drivers/barometer.h"
|
|
#include "drivers/arduino_com.h"
|
|
#include "drivers/beeper.h"
|
|
|
|
void systemTaskGyroPid(void)
|
|
{
|
|
|
|
//PID Gyro
|
|
pidRun(PID_ID_GYRO);
|
|
|
|
//call the motor mix
|
|
mix();
|
|
}
|
|
|
|
void systemTaskAccelerometer(void)
|
|
{
|
|
|
|
readAcc();
|
|
pidRun(PID_ID_ACCELEROMETER);
|
|
}
|
|
|
|
void systemTaskAttitude(void)
|
|
{
|
|
|
|
}
|
|
|
|
#define GET_CHANNEL_VALUE(id) { \
|
|
frame.chan##id \
|
|
}
|
|
|
|
void systemTaskRx(void)
|
|
{
|
|
//Interpret commands to the vehicle
|
|
sbus_read();
|
|
sbusFrame_s frame = sbusChannelData;
|
|
|
|
/* Process channel data for switches and toggles on the controller, starts after "stick" channels */
|
|
// for (int i = STICK_CHANNEL_COUNT; i < STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT; i++) //ToDo: add define for the number of channels to process, /if not work change i to start at numb of stick channels
|
|
// {
|
|
// flags_ProcessRcChannel(i+1, getChannelValue(frame, i+1));
|
|
// }
|
|
|
|
/*Updated flag processRcChannel function, not yet tested. Should work as the entire loop above*/
|
|
//flags_ProcessRcChannel_Improved(STICK_CHANNEL_COUNT+1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
|
|
|
|
/* Includes the stick channel in the toggles checks */
|
|
flags_ProcessRcChannel_Improved(1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
|
|
|
|
//temporary send data from the RC directly form the RC
|
|
// RawRcCommand.Roll = frame.chan1;
|
|
// RawRcCommand.Pitch = frame.chan2;
|
|
// RawRcCommand.Yaw = frame.chan4;
|
|
// RawRcCommand.Throttle = frame.chan4;
|
|
|
|
/* -- Check failsafe for RX -- */
|
|
//check no received message
|
|
(frame.flag_Failsafe) ? flags_Set_ID(systemFlags_Failsafe_noRcReceived_id) : flags_Clear_ID(systemFlags_Failsafe_noRcReceived_id);
|
|
|
|
//check missed frames
|
|
static int continuousMissedFrames = 0;
|
|
continuousMissedFrames = (frame.flag_FrameLost) ? continuousMissedFrames + 1 : 0;
|
|
(continuousMissedFrames > 10) ? flags_Set_ID(systemFlags_Failsafe_toManyMissedFrames_id) : flags_Clear_ID(systemFlags_Failsafe_toManyMissedFrames_id);
|
|
|
|
}
|
|
|
|
bool systemTaskRxCheck(uint32_t currentDeltaTime)
|
|
{
|
|
//This task is what is controlling the event activation of the systemTaskRx
|
|
//check if there is anything that has be received.
|
|
const uint32_t maxReceiveTrigger = 3000000; //3 seconds
|
|
static uint32_t lastRecievedCommand = 0;
|
|
|
|
|
|
|
|
bool isReady = sbus_frame_available();
|
|
// if (isReady == true)
|
|
// {
|
|
// flags_Clear_ID(systemFlags_Failsafe_noRcReceived_id);
|
|
// lastRecievedCommand = clock_get_us();
|
|
// return isReady;
|
|
// }
|
|
// else
|
|
// {
|
|
// //check for failsafe
|
|
// if ((clock_get_us() - lastRecievedCommand) > maxReceiveTrigger)
|
|
// {
|
|
// flags_Set_ID(systemFlags_Failsafe_noRcReceived_id);
|
|
// }
|
|
//
|
|
// return isReady;
|
|
// }
|
|
return isReady;
|
|
|
|
}
|
|
|
|
void systemTaskRxCli(void)
|
|
{
|
|
/* Check if CLI should be activated */
|
|
if (cliShouldRun() == true)
|
|
cliRun();
|
|
}
|
|
|
|
bool systemTaskRxCliCheck(uint32_t currentDeltaTime)
|
|
{
|
|
/* First check if any value has been sent to the cli usart.
|
|
* We dont care about the delta time for this since if this
|
|
* has received something we should always check we dont care about
|
|
* the loop times. */
|
|
return cliHasMessage();
|
|
|
|
return false;
|
|
}
|
|
|
|
//TODO: change the name of this task. Could be something like
|
|
void systemTaskSerial(void)
|
|
{
|
|
static bool readyToCalibrate = true;
|
|
const float calibrationAmount = 0.5;
|
|
|
|
//Only run if the system is not armed
|
|
if (!flags_IsSet_ID(systemFlags_armed_id))
|
|
{
|
|
if (flags_IsSet_ID(systemFlags_throttleMax_id))
|
|
{
|
|
// if(flags_IsSet_ID(systemFlags_throttleLeft_id))
|
|
// {
|
|
// if(flags_IsSet_ID(systemFlags_stickDown_id))
|
|
// {
|
|
// mpu6000_read_acc_offset(&accelProfile);
|
|
// }
|
|
// }
|
|
|
|
//If we are ready to accept a new calibration value. You can only perform one calibration until the sticks have been centered once until the next calibration
|
|
if(readyToCalibrate)
|
|
{
|
|
//If any calibration is performed set readyToCalibrate to false so it cant just increase indefinitely when holding the sticks in a certain position
|
|
if (flags_IsSet_ID(systemFlags_stickLeft_id))
|
|
{
|
|
accRollFineTune -= calibrationAmount;
|
|
readyToCalibrate = false;
|
|
}
|
|
else if (flags_IsSet_ID(systemFlags_stickRight_id))
|
|
{
|
|
accRollFineTune += calibrationAmount;
|
|
readyToCalibrate = false;
|
|
}
|
|
else if (flags_IsSet_ID(systemFlags_stickUp_id))
|
|
{
|
|
accPitchFineTune -= calibrationAmount;
|
|
readyToCalibrate = false;
|
|
}
|
|
else if (flags_IsSet_ID(systemFlags_stickDown_id))
|
|
{
|
|
accPitchFineTune += calibrationAmount;
|
|
readyToCalibrate = false;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
//if the stick is centered set ready to calibrate to true
|
|
if(flags_IsSet_MASK(systemFlags_stickCenterH_mask | systemFlags_stickCenterV_mask))
|
|
{
|
|
readyToCalibrate = true;
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void systemTaskBattery(void)
|
|
{
|
|
// uint8_t c = 118;
|
|
// usart_transmit(&cliUsart, &c, 1, 1000000000);
|
|
if (flags_IsSet_ID(systemFlags_armed_id))
|
|
ledOnInverted(Led0_PIN, Led0_GPIO_PORT);
|
|
else
|
|
ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
|
|
}
|
|
|
|
void systemTaskArduino(void)
|
|
{
|
|
arduino_read();
|
|
arduino_send_sensor_values();
|
|
}
|
|
|
|
void systemTaskBaro(void)
|
|
{
|
|
barometer_CaclulateValues();
|
|
}
|
|
|
|
void systemTaskCompass(void)
|
|
{
|
|
pidRun(PID_ID_COMPASS);
|
|
}
|
|
|
|
void systemTaskGps(void)
|
|
{
|
|
//Obtain gps data
|
|
}
|
|
|
|
void systemTaskSonar(void)
|
|
{
|
|
//obtain sonar data
|
|
}
|
|
|
|
void systemTaskAltitude(void)
|
|
{
|
|
// uint8_t c[50];
|
|
// sprintf(c, "Roll: %-6f, Pitch %-6f\r", accelProfile.rollAngle, accelProfile.pitchAngle);
|
|
// usart_transmit(&cliUsart, &c, 50, 1000000000);
|
|
|
|
//Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar
|
|
|
|
//double temperature = barometer_GetCurrentTemperature();
|
|
//double pressure = barometer_GetCurrentPreassure();
|
|
//float altitute = barometer_GetCurrentAltitude();
|
|
//float altitute = barometer_GetCurrentAveragedtAltitude();
|
|
|
|
//pid run, should probably be moved to systemTaskAltitude
|
|
pidRun(PID_ID_BAROMETER);
|
|
}
|
|
|
|
void systemTaskBeeper(void)
|
|
{
|
|
|
|
}
|
|
|
|
/* TO BE USED ONLY WHEN TESTING/DEBUGIN TASK FUNCTIONALLITY, DONT USE WHEN RUNNING THE REAL SYSTEM!!!!!!!!!! */
|
|
#ifdef USE_DEBUG_TASKS
|
|
|
|
void systemTaskDebug_1(void)
|
|
{
|
|
//ledToggle(Led0_PIN, Led0_GPIO_PORT);
|
|
clock_delay_ms(8);
|
|
//ledToggle(Led0_PIN, Led0_GPIO_PORT);
|
|
}
|
|
|
|
void systemTaskDebug_2(void)
|
|
{
|
|
//ledToggle(Led1, Led1_GPIO_PORT);
|
|
//clock_delay_ms(15);
|
|
clock_delay_ms(8);
|
|
//ledToggle(Led1, Led1_GPIO_PORT);
|
|
}
|
|
|
|
void systemTaskDebug_3(void)
|
|
{
|
|
//ledToggle(GPIO_PIN_0, GPIOA);
|
|
//clock_delay_ms(20);
|
|
clock_delay_ms(8);
|
|
//ledToggle(GPIO_PIN_0, GPIOA);
|
|
}
|
|
|
|
#endif /* End USE_DEBUG_TASKS */
|