MDH-UAV-Control-System/UAV-ControlSystem/src/tasks_main.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"

void systemTaskGyroPid(void)
{
	//Read gyro and update PID and finally update the motors. The most important task in the system

	//Update Gyro

	//Convert?

	//PID Gyro
	pidRun(PID_ID_GYRO);

	//MIX GO


	//call the motor mix
	mix();
}

void systemTaskAccelerometer(void)
{

	pidRun(PID_ID_ACCELEROMETER);
	//update the accelerometer data
//	uint8_t c = 97;
//	usart_transmit(&cliUsart, &c, 1, 1000000000);
}

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);

	//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;
}

void systemTaskSerial(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 systemTaskBattery(void)
{
	//Keep track of the battery level of the system
//	uint8_t c = 98;
//	usart_transmit(&cliUsart, &c, 1, 1000000000);
	if (flags_IsSet_MASK((systemFlags_flightmode_acceleromter_mask | systemFlags_armed_mask)))
		ledOnInverted(Led1, Led1_GPIO_PORT);
	else
		ledOffInverted(Led1, Led1_GPIO_PORT);
}

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)
{
	//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_GetCurrentAltitudeBasedOnSeaLevel();

	//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 */