From 2b05c843a15e618bfdc04a60fa407aab29b0bcd5 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Wed, 12 Oct 2016 14:58:21 +0200 Subject: [PATCH] Fiexed small errors and added more commands for RC things in CLI Also added some statistical values that can be veiwed in CLI to see how they sytem performs. --- UAV-ControlSystem/inc/Scheduler/scheduler.h | 14 ++ UAV-ControlSystem/inc/config/eeprom.h | 5 +- .../inc/drivers/failsafe_toggles.h | 8 +- UAV-ControlSystem/inc/drivers/motormix.h | 1 + UAV-ControlSystem/src/Scheduler/scheduler.c | 17 +- UAV-ControlSystem/src/config/cli.c | 148 ++++++++++++++++-- UAV-ControlSystem/src/config/eeprom.c | 14 +- .../src/drivers/failsafe_toggles.c | 4 +- UAV-ControlSystem/src/drivers/motormix.c | 13 +- UAV-ControlSystem/src/main.c | 5 +- UAV-ControlSystem/src/tasks_main.c | 31 +++- 11 files changed, 225 insertions(+), 35 deletions(-) diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h index ce9186c..945f953 100644 --- a/UAV-ControlSystem/inc/Scheduler/scheduler.h +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -74,12 +74,25 @@ typedef struct uint32_t averageExecutionTime; // Moving average over 6 samples, used to calculate guard interval uint32_t taskLatestDeltaTime; // #ifndef SKIP_TASK_STATISTICS + uint32_t taskAverageDeltaTime; uint32_t maxExecutionTime; uint32_t totalExecutionTime; // total time consumed by task since boot #endif }task_t; +/* Struct used to store statistical data of the scheduler */ +typedef struct +{ + uint32_t totalSystemRuntime; + uint32_t totalTaskRuntime; //Total time spent for tasks running + uint32_t totalOverHead; //Total overhead when selecting a task + uint32_t averageOverHead; + uint32_t totalMissedPeriods; //Number of time any task have missed a period + uint32_t totalSchedulerCalls; //amount of times scheduler have been called + uint32_t totalTasksScheduled; //the total amount of tasks schedulede +}scheduler_statistics_t; + /* Task counter, ToDo: If more tasks are needed add them here first, defines are used to make sure the task is somewhere in the system */ /* Important: If a new define for a task is added here it MUST be added in the tasks.c */ @@ -129,6 +142,7 @@ typedef enum }taskId_t; /* Variables -------------------------------------------------------------*/ +extern scheduler_statistics_t systemSchedulerStatistics; extern task_t SystemTasks[TASK_COUNT]; /* All the tasks that exist in the system */ extern uint16_t averageSystemLoadPercent; /* The average load on the system Todo */ diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 2afd765..1af16b4 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -175,13 +175,16 @@ typedef enum { EEPROM_PERIOD_BEEPER, #endif + /* Motormix values */ + EEPROM_MOTORMIX_CONFIG, + /* Flags eeprom values */ EEPROM_FLAG_ARM, EEPROM_FLAG_FLIGHTMODE_ACCELEROMETER, EEPROM_FLAG_FLIGHTMODE_BAROMETER, EEPROM_FLAG_FLIGHTMODE_COMPASS, EEPROM_FLAG_FLIGHTMODE_GPS, - EEPROM_FLAG_FLIGHTMODE_1, + EEPROM_FLAG_AIRMODE, EEPROM_FLAG_FLIGHTMODE_2, EEPROM_FLAG_FLIGHTMODE_3, diff --git a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h index 17dc718..3cbaf91 100644 --- a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h +++ b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h @@ -27,7 +27,7 @@ #define systemFlags_flightmode_barometer_id 2 + boolean_vals_offset #define systemFlags_flightmode_compass_id 3 + boolean_vals_offset #define systemFlags_flightmode_gps_id 4 + boolean_vals_offset -#define systemFlags_flightMode_1_id 5 + boolean_vals_offset +#define systemFlags_airmode_id 5 + boolean_vals_offset #define systemFlags_flightMode_2_id 6 + boolean_vals_offset #define systemFlags_flightMode_3_id 7 + boolean_vals_offset @@ -43,7 +43,7 @@ #define systemFlags_flightmode_barometer_mask getFlagMaskValue(systemFlags_flightmode_barometer_id) #define systemFlags_flightmode_compass_mask getFlagMaskValue(systemFlags_flightmode_compass_id) #define systemFlags_flightmode_gps_mask getFlagMaskValue(systemFlags_flightmode_gps_id) -#define systemFlags_flightMode_1_mask getFlagMaskValue(systemFlags_flightMode_1_id) +#define systemFlags_airmode_mask getFlagMaskValue(systemFlags_airmode_id) #define systemFlags_flightMode_2_mask getFlagMaskValue(systemFlags_flightMode_2_id) #define systemFlags_flightMode_3_mask getFlagMaskValue(systemFlags_flightMode_3_id) @@ -67,7 +67,7 @@ typedef union bitSetRegion booleanValue_t barometer : 1; booleanValue_t compass : 1; booleanValue_t gps : 1; - booleanValue_t flightMode_1 : 1; + booleanValue_t airmode : 1; booleanValue_t flightMode_2 : 1; booleanValue_t flightMode_3 : 1; }bitField; @@ -90,7 +90,7 @@ typedef enum FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER, FLAG_CONFIGURATION_FLIGHTMODE_COMPASS, FLAG_CONFIGURATION_FLIGHTMODE_GPS, - FLAG_CONFIGURATION_FLIGHTMODE_1, + FLAG_CONFIGURATION_AIRMODE, FLAG_CONFIGURATION_FLIGHTMODE_2, FLAG_CONFIGURATION_FLIGHTMODE_3, diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h index c01139a..498bb8d 100644 --- a/UAV-ControlSystem/inc/drivers/motormix.h +++ b/UAV-ControlSystem/inc/drivers/motormix.h @@ -43,6 +43,7 @@ typedef struct { uint16_t minCommand; // Pulse when motors are running idle (Armed) uint16_t maxCommand; // Max throttle allowed. Mixer can go higher than this though. uint16_t minCheck; // In Non Airmode: If throttle is below minCheck we set motors to minCommand + uint16_t padding; bool pid_at_min_throttle; // When enabled PIDs are used at minimum throttle bool motorstop; // If enabled motors will stop spinning at no throttle when Armed bool yaw_reverse_direction; // Default should be 1. Can be either -1 or 1 diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index c85e195..340687a 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -47,6 +47,7 @@ static uint32_t tasksExecutionTimeUs = 0; //Total execution time of the task //static uint32_t lastSystemLoadTimeValUs = 0; //Not used right now, would be used to calculate load with time vals uint32_t taskAgeCycleStatisitcs[taskAgeCycleCounterSize]; //Age cycle statistics array +scheduler_statistics_t systemSchedulerStatistics = {0}; //Struct holding scheduler statisitcs /* Functions to operate on the task queue ------------------------------------------------------- */ @@ -451,7 +452,10 @@ void scheduler(void) #ifdef USE_TASK_AGE_CYCLE_STATISTICS if (taskToRun->taskAgeCycle > 1) + { taskAgeCycleStatisitcs[taskToRun->taskAgeCycle] ++; //increment the statistic counter for age cycles if we miss period + systemSchedulerStatistics.totalMissedPeriods ++; + } #endif #ifdef USE_LED_WARNINGS_MISSED_PERIOD if (taskToRun->taskAgeCycle > 1) @@ -514,6 +518,12 @@ void scheduler(void) //Assign the selected task to the pointer for the current task currentTask = selectedTask; + //Calculate the overhead to select a new task + uint32_t currentOverHead = (clock_get_us() - currentTime); + systemSchedulerStatistics.totalOverHead += currentOverHead; + systemSchedulerStatistics.averageOverHead = ((uint32_t)systemSchedulerStatistics.averageOverHead * 31 + currentOverHead) / 32; + systemSchedulerStatistics.totalSchedulerCalls ++; + //If we have found a task to run, execute the function that the task is responsible for if (selectedTask != NULL) { @@ -530,11 +540,16 @@ void scheduler(void) tasksExecutionTimeUs += taskExecutionTime; //Add the task execution time for each task execution, will be used to //Save statistical values + selectedTask->taskAverageDeltaTime = ((uint32_t)selectedTask->taskAverageDeltaTime * 31 + selectedTask->taskLatestDeltaTime) / 32; selectedTask->averageExecutionTime = ((uint32_t)selectedTask->averageExecutionTime * 31 + taskExecutionTime) / 32; selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); - } + //statistical values for the scheduler + systemSchedulerStatistics.totalTaskRuntime += taskExecutionTime; + systemSchedulerStatistics.totalTasksScheduled ++; + } + systemSchedulerStatistics.totalSystemRuntime = clock_get_us(); } diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 1fe8df6..b6338fa 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -22,6 +22,7 @@ #include #include #include "utilities.h" +#include "Scheduler/scheduler.h" #define cliActivateCharacter 35 #define commandValueError 0xFFFFFFFFFFFFFFFF @@ -69,6 +70,7 @@ typedef enum ACTION_EXIT, //Exists the cli ACTION_REBOOT, //Reboots the entire system ACTION_RESET, //Resets the entire eeprom + ACTION_STATS, //gives statistics on the system //The number of actions ACTION_COUNT, //Count of the number of actions @@ -88,7 +90,8 @@ const typeString commandAction_Strings[ACTION_COUNT] = { "profile", "exit", "reboot", - "reset" + "reset", + "stats" }; /* String values descrbing information of a certain action */ @@ -103,7 +106,8 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = { "| profile - Changes the active profile between the values (1-3).\n\r", "| exit - Exits the CLI mode.\n\r", "| reboot - Exit CLI and reboots the system.\n\r", - "| reset - Restore all the values to its default values-\n\r" + "| reset - Restore all the values to its default values.\n\r", + "| stats - Gives some current stats of the system and tasks.\n\r" }; /* String array containing all the signature examples for each action */ @@ -118,7 +122,8 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = { " profile number", " exit", " reboot", - " reset" + " reset", + " stats" }; /* Size values for the values a command will require */ @@ -131,7 +136,8 @@ typedef enum VAL_UINT_32, VAL_INT_32, VAL_UINT_64, - VAL_INT_64 + VAL_INT_64, + VAL_BOOL }valueTypes_t; /* ID values for individual command, all commands must have a uniqe ID value */ @@ -170,6 +176,18 @@ typedef enum COMMAND_ID_PERIOD_BEEPER, #endif + + /* Commands for chaning motormixer values */ + COMMAND_ID_MOTORMIX_MINTHROTTLE, + COMMAND_ID_MOTORMIX_MAXTHROTTLE, + COMMAND_ID_MOTORMIX_MINCOMMAND, + COMMAND_ID_MOTORMIX_MAXCOMMAND, + COMMAND_ID_MOTORMIX_MINCHECK, + COMMAND_ID_MOTORMIX_PID_AT_MINTHROTTLE, + COMMAND_ID_MOTORMIX_MOTORSTOP, + COMMAND_ID_MOTORMIX_YAW_REVERSE_DIRECTION, + + /* Commands for changing flag toggles from RC */ COMMAND_ID_FLAG_ARM_MINRANGE, COMMAND_ID_FLAG_ARM_MAXRANGE, @@ -191,9 +209,9 @@ typedef enum COMMAND_ID_FLAG_FLIGHTMODE_GPS_MAXRANGE, COMMAND_ID_FLAG_FLIGHTMODE_GPS_CHANNEL, - COMMAND_ID_FLAG_FLIGHTMODE_1_MINRANGE, - COMMAND_ID_FLAG_FLIGHTMODE_1_MAXRANGE, - COMMAND_ID_FLAG_FLIGHTMODE_1_CHANNEL, + COMMAND_ID_FLAG_AIRMODE_MINRANGE, + COMMAND_ID_FLAG_AIRMODE_MAXRANGE, + COMMAND_ID_FLAG_AIRMODE_CHANNEL, COMMAND_ID_FLAG_FLIGHTMODE_2_MINRANGE, COMMAND_ID_FLAG_FLIGHTMODE_2_MAXRANGE, @@ -203,8 +221,11 @@ typedef enum COMMAND_ID_FLAG_FLIGHTMODE_3_MAXRANGE, COMMAND_ID_FLAG_FLIGHTMODE_3_CHANNEL, + + /* Counter for the amount of commands */ COMMAND_ID_COUNT, + /* ID number for a non existing commands */ COMMAND_ID_NO_COMMAND }command_Ids_t; @@ -318,6 +339,41 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { #endif + /* Commands for chaning motormixer values */ + [COMMAND_ID_MOTORMIX_MINTHROTTLE] = + { + "motormix_minthrottle", COMMAND_ID_MOTORMIX_MINTHROTTLE, EEPROM_MOTORMIX_CONFIG, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_16, .valueRange = {0, 2500} + }, + [COMMAND_ID_MOTORMIX_MAXTHROTTLE] = + { + "motormix_maxthrottle", COMMAND_ID_MOTORMIX_MAXTHROTTLE, EEPROM_MOTORMIX_CONFIG, EEPROM_VALUE_TYPE_SYSTEM, 2, VAL_UINT_16, .valueRange = {0, 2500} + }, + [COMMAND_ID_MOTORMIX_MINCOMMAND] = + { + "motormix_mincommand", COMMAND_ID_MOTORMIX_MINCOMMAND, EEPROM_MOTORMIX_CONFIG, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_16, .valueRange = {0, 2500} + }, + [COMMAND_ID_MOTORMIX_MAXCOMMAND] = + { + "motormix_maxcommand", COMMAND_ID_MOTORMIX_MAXCOMMAND, EEPROM_MOTORMIX_CONFIG, EEPROM_VALUE_TYPE_SYSTEM, 6, VAL_UINT_16, .valueRange = {0, 2500} + }, + [COMMAND_ID_MOTORMIX_MINCHECK] = + { + "motormix_mincheck", COMMAND_ID_MOTORMIX_MINCHECK, EEPROM_MOTORMIX_CONFIG, EEPROM_VALUE_TYPE_SYSTEM, 8, VAL_UINT_16, .valueRange = {0, 2500} + }, + [COMMAND_ID_MOTORMIX_PID_AT_MINTHROTTLE] = + { + "motormix_pid_at_minthrottle", COMMAND_ID_MOTORMIX_PID_AT_MINTHROTTLE, EEPROM_MOTORMIX_CONFIG, EEPROM_VALUE_TYPE_SYSTEM, 12, VAL_BOOL, .valueRange = {0, 1} + }, + [COMMAND_ID_MOTORMIX_MOTORSTOP] = + { + "motormix_motorstop", COMMAND_ID_MOTORMIX_MOTORSTOP, EEPROM_MOTORMIX_CONFIG, EEPROM_VALUE_TYPE_SYSTEM, 16, VAL_BOOL, .valueRange = {0, 1} + }, + [COMMAND_ID_MOTORMIX_YAW_REVERSE_DIRECTION] = + { + "motormix_yaw_reverse_direction", COMMAND_ID_MOTORMIX_YAW_REVERSE_DIRECTION, EEPROM_MOTORMIX_CONFIG, EEPROM_VALUE_TYPE_SYSTEM, 20, VAL_BOOL, .valueRange = {0, 1} + }, + + /* Commands for changing flag toggles from RC */ [COMMAND_ID_FLAG_ARM_MINRANGE] = { @@ -384,17 +440,17 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { "flag_flightmode_gps_channel", COMMAND_ID_FLAG_FLIGHTMODE_GPS_CHANNEL, EEPROM_FLAG_FLIGHTMODE_GPS, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18} }, - [COMMAND_ID_FLAG_FLIGHTMODE_1_MINRANGE] = + [COMMAND_ID_FLAG_AIRMODE_MINRANGE] = { - "flag_flightmode_1_minrange", COMMAND_ID_FLAG_FLIGHTMODE_1_MINRANGE, EEPROM_FLAG_FLIGHTMODE_1, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_16, .valueRange = {0, 2500} + "flag_airmode_minrange", COMMAND_ID_FLAG_AIRMODE_MINRANGE, EEPROM_FLAG_AIRMODE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_16, .valueRange = {0, 2500} }, - [COMMAND_ID_FLAG_FLIGHTMODE_1_MAXRANGE] = + [COMMAND_ID_FLAG_AIRMODE_MAXRANGE] = { - "flag_flightmode_1_maxrange", COMMAND_ID_FLAG_FLIGHTMODE_1_MAXRANGE, EEPROM_FLAG_FLIGHTMODE_1, EEPROM_VALUE_TYPE_SYSTEM, 2, VAL_UINT_16, .valueRange = {0, 2500} + "flag_airmode_maxrange", COMMAND_ID_FLAG_AIRMODE_MAXRANGE, EEPROM_FLAG_AIRMODE, EEPROM_VALUE_TYPE_SYSTEM, 2, VAL_UINT_16, .valueRange = {0, 2500} }, - [COMMAND_ID_FLAG_FLIGHTMODE_1_CHANNEL] = + [COMMAND_ID_FLAG_AIRMODE_CHANNEL] = { - "flag_flightmode_1_channel", COMMAND_ID_FLAG_FLIGHTMODE_1_CHANNEL, EEPROM_FLAG_FLIGHTMODE_1, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18} + "flag_airmode_channel", COMMAND_ID_FLAG_AIRMODE_CHANNEL, EEPROM_FLAG_AIRMODE, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18} }, [COMMAND_ID_FLAG_FLIGHTMODE_2_MINRANGE] = @@ -690,6 +746,9 @@ bool processCommand(uint8_t action, uint16_t id, uint64_t value) case VAL_INT_64: *((int64_t *)valuePointer) = value; break; + case VAL_BOOL: + *((bool *)valuePointer) = value; + break; } return true; @@ -723,6 +782,9 @@ void valuePointerToString(char * dst, uint16_t id, void * address) case VAL_INT_64: sprintf(dst,"%d", *((int64_t *)address)); break; + case VAL_BOOL: + sprintf(dst,"%d", *((bool *)address)); + break; } } @@ -921,7 +983,7 @@ void writeTaskInformation(uint16_t id) { //buffer char buffer[maxStringSize_CLI]; - char valBuffer[15]; + char valBuffer[16]; //Get the correct pointer to the data void * valuePointer = getDataAddresFromID(commandTable[id].valueId, commandTable[id].valueIdLoc); @@ -934,6 +996,61 @@ void writeTaskInformation(uint16_t id) TransmitBack("\n\r"); } +/*********************************************************************** +* BRIEF: Will transmit some system stats over the usart +* INFORMATION: Will transmit some system stats, including scheduler +* data, individial task data potentially more. +***********************************************************************/ +void TransmitSystemStats() +{ + char * buffer[maxStringSize_CLI*2]; + float taskExecEfficiency = ((float)systemSchedulerStatistics.totalTaskRuntime/(float)systemSchedulerStatistics.totalSystemRuntime)*100.f; + float taskschedulingUptime = ((float)systemSchedulerStatistics.totalTasksScheduled/(float)systemSchedulerStatistics.totalSchedulerCalls)*100.f; + float averageOverHead = ((float)systemSchedulerStatistics.totalOverHead/(float)systemSchedulerStatistics.totalSchedulerCalls)*100.f; + //initial start message " + TransmitBack("- System statistics\n\r-------------------\n\n\r- Scheduler stats: \n\r"); + TransmitBack("------------------------------------------------------\n\r"); + sprintf(buffer,"- %-35s%-16d ms\n\r", "System runtime:", systemSchedulerStatistics.totalSystemRuntime/1000); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d ms\n\r", "Tasks runtime:", systemSchedulerStatistics.totalTaskRuntime/1000); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d ms\n\r", "Overhead:", systemSchedulerStatistics.totalOverHead/1000); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d micro\n\r", "Average Overhead:", ((int)taskschedulingUptime)); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d percent\n\r", "Task exec by runtime:", ((int)taskExecEfficiency)); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d times\n\r", "Scheduler calls:", systemSchedulerStatistics.totalSchedulerCalls); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d times\n\r", "Task scheduled:", systemSchedulerStatistics.totalTasksScheduled); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d percent\n\r", "Task schedule load:", ((int)taskschedulingUptime)); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d times\n\r", "Missed periods:", systemSchedulerStatistics.totalMissedPeriods); + TransmitBack(buffer); + + + TransmitBack("\n\n\r- Individual task stats: \n\r"); + TransmitBack("------------------------------------------------------\n\r"); + for (int i = 0; i < TASK_COUNT; i++) + { + sprintf(buffer,"- %-35s%s\n\r", "Task name: ", SystemTasks[i].taskName); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d ms\n\r", "Total execution time: ", SystemTasks[i].totalExecutionTime/1000); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d micro\n\r", "Max execution time: ", SystemTasks[i].maxExecutionTime); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d micro\n\r", "Average execution time: ", SystemTasks[i].averageExecutionTime); + TransmitBack(buffer); + sprintf(buffer,"- %-35s%-16d micro\n\r", "Average time between exec: ", SystemTasks[i].taskAverageDeltaTime); + TransmitBack(buffer); + + TransmitBack("\n\r"); + } + + +} + /*********************************************************************** * BRIEF: Will print informations of all the the commands * INFORMATION: Will loop through all the commands in the system and @@ -1139,6 +1256,9 @@ void cliRun() TransmitBack("- Values unchanged...\n\n\r"); } break; + case commandMask(commandSize_1, ACTION_STATS): + TransmitSystemStats(); + break; default: if (actionId != ACTION_NOACTION) TransmitCommandInstruction(actionId); diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index d42ca4f..15e85c1 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -22,6 +22,7 @@ #include "stm32f4xx_revo.h" #include "Scheduler/scheduler.h" #include "drivers/failsafe_toggles.h" +#include "drivers/motormix.h" /* Reads the EEPROM version from EEPROM - Is compared to EEPROM_SYS_VERSION */ uint8_t stored_eeprom_identifier; @@ -186,6 +187,15 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = { }, #endif + + /* Motormix values */ + [EEPROM_MOTORMIX_CONFIG] = + { + .size = sizeof(mixerConfig_s), + .dataPtr = &(mixerConfig), + }, + + /* Flags eeprom values */ [EEPROM_FLAG_ARM] = { @@ -212,10 +222,10 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = { .size = sizeof(flags_Configuration_t), .dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_GPS]), }, - [EEPROM_FLAG_FLIGHTMODE_1] = + [EEPROM_FLAG_AIRMODE] = { .size = sizeof(flags_Configuration_t), - .dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_1]), + .dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_AIRMODE]), }, [EEPROM_FLAG_FLIGHTMODE_2] = { diff --git a/UAV-ControlSystem/src/drivers/failsafe_toggles.c b/UAV-ControlSystem/src/drivers/failsafe_toggles.c index b3778c6..d9056f5 100644 --- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c +++ b/UAV-ControlSystem/src/drivers/failsafe_toggles.c @@ -42,11 +42,11 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = { .channelNumber = 0, .flagId = systemFlags_flightmode_gps_id, }, - [FLAG_CONFIGURATION_FLIGHTMODE_1] = { + [FLAG_CONFIGURATION_AIRMODE] = { .minRange = 0, .maxRange = 0, .channelNumber = 0, - .flagId = systemFlags_flightMode_1_id, + .flagId = systemFlags_airmode_id, }, [FLAG_CONFIGURATION_FLIGHTMODE_2] = { .minRange = 0, diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index ce4b9e0..574ecff 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -19,6 +19,7 @@ #include "drivers/motors.h" #include "utilities.h" #include "drivers/sbus.h" +#include "drivers/failsafe_toggles.h" /* An illustration of the motor configuration @@ -35,7 +36,7 @@ /* Set by EEPROM - This variable decides whether the control * system should be active or not when throttle is below min_throttle */ -bool pid_at_min_throttle = true; +//bool pid_at_min_throttle = true; /* An array containing the calculated motor outputs */ uint16_t motor_output[MOTOR_COUNT]; @@ -43,12 +44,14 @@ uint16_t motor_output[MOTOR_COUNT]; /* Default values for the mixerConfig */ // TODO: Implement in EEPROM mixerConfig_s mixerConfig = { - .yaw_motor_direction = 1, .minThrottle = 1000, .maxThrottle = 2000, .minCommand = 1050, .maxCommand = 1950, - .minCheck = 1010 + .minCheck = 1010, + .pid_at_min_throttle = true, + .motorstop = false, + .yaw_reverse_direction = false }; /* Used in "mixerUAV" to create the dynamic model of the UAV */ @@ -101,7 +104,7 @@ void mix() // Might be used for some debug if necessary static bool motorLimitReached; - if (false) // TODO: replace with check for Airmode + if (flags_IsSet_ID(systemFlags_airmode_id)) // TODO: replace with check for Airmode { for (int i = 0; i < MOTOR_COUNT; i++) { @@ -206,7 +209,7 @@ void mix() for (int i = 0; i < MOTOR_COUNT; i++) { /* If engines are armed then give the output to the motors */ - if (true) // TODO: replace with check for armed (IF ARMED) + if (flags_IsSet_ID(systemFlags_armed_id)) // TODO: replace with check for armed (IF ARMED) motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand); /* If not then stop motors */ else diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 116ab49..af2da2d 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -25,6 +25,7 @@ #include "config/eeprom.h" #include "drivers/failsafe_toggles.h" #include "drivers/sbus.h" +#include "drivers/motormix.h" /************************************************************************** * BRIEF: Should contain all the initializations of the system, needs to @@ -47,10 +48,10 @@ void init_system() //initialize the CLI - cliInit(USART6); + cliInit(USART1); //init sbus - sbus_init(); + //sbus_init(); #ifdef USE_LEDS //Initialize the on board leds diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 522648e..7ea0c8f 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -43,6 +43,10 @@ void systemTaskGyroPid(void) { //Read gyro and update PID and finally update the motors. The most important task in the system + + uint8_t c = 102; + if (flags_IsSet_ID(systemFlags_Failsafe_noRcReceived_id)) + usart_transmit(&cliUsart, &c, 1, 1000000000); } void systemTaskAccelerometer(void) @@ -68,7 +72,7 @@ void systemTaskRx(void) 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 + for (int i = 1; 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)); } @@ -79,10 +83,29 @@ 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; + } - flags_Set_ID(systemFlags_Failsafe_noRcReceived_id); - //ToDo: add failsafe for no command revieved - return sbus_frame_available(); } void systemTaskRxCli(void)