diff --git a/UAV-ControlSystem/inc/config/cli.h b/UAV-ControlSystem/inc/config/cli.h index c686d3b..8492df7 100644 --- a/UAV-ControlSystem/inc/config/cli.h +++ b/UAV-ControlSystem/inc/config/cli.h @@ -55,5 +55,50 @@ bool cliHasMessage(); bool cliShouldRun(); - #endif /* CONFIG_CLI_H_ */ + +/* CHECKLIST - How to add new command in CLI */ +/* A: First add the value that should be changed to the EEPROM + * + * 1: Create a EEPROM enum id for the value in eeprom.h. Make sure + * to place within the correct enum container system or profile. + * + * 2: In eeprom.c add two values to the correct array for the value + * profile or system. THe two values should be the pointer to the + * value to store in eeprom and the size of said value. + * + * 3: Now the value should be storable int the EEPROM. + * + * B: Create the actual command in the CLI. + * + * 1: First create an enum id for the CLI command in the command_Ids_t + * contained in the cli.c. + * + * 2: When the id is created it represents an array value in the + * commandTable, an array of cliCommandConfig_t. + * + * 3: Initialize the values of the cliCommandConfig_t for the newly + * created command id. Provide in this order: + * - The name of the command when typing it in CLI. + * - The created Command id. + * - The EEPROM id to the value the command shoud operate on. + * - The type of EEPROM value syetem, profile, etc... + * - The offset of the value at the given eeprom address, usable if + * the value stored in the EEPROM is for example a struct and + * the value to be accessed is for example 4 bytes from the start + * of the struct. In that case this value would be set to 4. + * - The type of the value the command should operate on. Uint8, + * uint16, uint 32 etc... + * - The min and maximum value that can be given using the command. + * + * C: When both these things have been done the command should have been + * added to the CLI and it should operate on the correct value in the + * system that is also stored in the EEPROM. + * + * D: NOTE, be careful when setting up the CLI command, any small mistake + * in the command can cause strange errors that can be hard to track + * down at a later stage. Therefore make sure that the command works + * as intended and double check all the values for the command and see + * that they are what they should be. + * */ + diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 1af16b4..06536a4 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -147,6 +147,9 @@ typedef enum { EEPROM_ADC_SCALES, EEPROM_UART1_RX_INV, + /* Motor calibrate bool */ + EEPROM_MOTORCALIBRATE, + /* Period values for tasks */ EEPROM_PERIOD_SYSTEM, EEPROM_PERIOD_GYROPID, @@ -184,8 +187,8 @@ typedef enum { EEPROM_FLAG_FLIGHTMODE_BAROMETER, EEPROM_FLAG_FLIGHTMODE_COMPASS, EEPROM_FLAG_FLIGHTMODE_GPS, - EEPROM_FLAG_AIRMODE, - EEPROM_FLAG_FLIGHTMODE_2, + EEPROM_FLAG_MIXERFULLSCALE, + EEPROM_FLAG_MIXERLOWSCALE, EEPROM_FLAG_FLIGHTMODE_3, /* Counts the amount of system settings */ diff --git a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h index 3cbaf91..2c6e72f 100644 --- a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h +++ b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h @@ -1,9 +1,23 @@ -/* - * failsafe_toggles.h - * - * Created on: 10 okt. 2016 - * Author: holmis - */ + /********************************************************************** + * NAME: failsafe_toggles.h * + * AUTHOR: Jonas Holmberg * + * PURPOSE: Give the ability to create boolean flags and failsafe * + * variables. * + * INFORMATION: Create any boolean variable that should be able to be * + * used throughout the system. These can be boolean vales * + * for standard checks, failsafes etc. These can also be * + * linked to a RC channel and value range wherein the * + * value will be set to one. * + * * + * GLOBAL VARIABLES: * + * Variable Type Description * + * -------- ---- ----------- * + * systemFlags boolFlags_t Represents a bitfield of all the * + * boolean values in the system. * + * flagConfigArr flags_Configuration_t Configuration values for any * + * flag that should be changeable * + * from the RC controller. * + **********************************************************************/ #ifndef DRIVERS_FAILSAFE_TOGGLES_H_ #define DRIVERS_FAILSAFE_TOGGLES_H_ @@ -27,8 +41,8 @@ #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_airmode_id 5 + boolean_vals_offset -#define systemFlags_flightMode_2_id 6 + boolean_vals_offset +#define systemFlags_mixerfullscale_id 5 + boolean_vals_offset +#define systemFlags_mixerlowscale_id 6 + boolean_vals_offset #define systemFlags_flightMode_3_id 7 + boolean_vals_offset @@ -43,8 +57,8 @@ #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_airmode_mask getFlagMaskValue(systemFlags_airmode_id) -#define systemFlags_flightMode_2_mask getFlagMaskValue(systemFlags_flightMode_2_id) +#define systemFlags_mixerfullscale_mask getFlagMaskValue(systemFlags_mixerfullscale_id) +#define systemFlags_mixerlowscale_mask getFlagMaskValue(systemFlags_mixerlowscale_id) #define systemFlags_flightMode_3_mask getFlagMaskValue(systemFlags_flightMode_3_id) @@ -67,8 +81,8 @@ typedef union bitSetRegion booleanValue_t barometer : 1; booleanValue_t compass : 1; booleanValue_t gps : 1; - booleanValue_t airmode : 1; - booleanValue_t flightMode_2 : 1; + booleanValue_t mixerfullscale : 1; + booleanValue_t mixerlowscale : 1; booleanValue_t flightMode_3 : 1; }bitField; uint64_t intRepresentation; @@ -90,77 +104,91 @@ typedef enum FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER, FLAG_CONFIGURATION_FLIGHTMODE_COMPASS, FLAG_CONFIGURATION_FLIGHTMODE_GPS, - FLAG_CONFIGURATION_AIRMODE, - FLAG_CONFIGURATION_FLIGHTMODE_2, + FLAG_CONFIGURATION_MIXERFULLSCALE, + FLAG_CONFIGURATION_MIXERLOWSCALE, FLAG_CONFIGURATION_FLIGHTMODE_3, //Counter FLAG_CONFIGURATION_COUNT }flags_configuration_IDs_t; -/* Stuct containing values for all the flags and failsafe booleans sin the system */ +/* Bitfield containing values for all the flags and failsafe booleans sin the system */ extern boolFlags_t systemFlags; /* Array of flag configurations. These are values that can be set by RC. */ extern flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT]; /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Could be used to set start values for some values +* INFORMATION: Possible to set the values for any of the failsafes ***********************************************************************/ void initFlags(); /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Process RC channels that should operate on some flags +* INFORMATION: Reads the value of the RC channels that and checks if +* any of the channels should handle some flag value in the +* system. If it should it will update its state. ***********************************************************************/ void flags_ProcessRcChannel(int rcChannel_ID, int value); /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Process RC channels that should operate on some flags +* INFORMATION: Reads the value of the RC channels that and checks if +* any of the channels should handle some flag value in the +* system. If it should it will update its state. +***********************************************************************/ +void flags_ProcessRcChannel_Improved(uint8_t minChannel, uint8_t maxChannel); + +/*********************************************************************** +* BRIEF: Set flag to true(1) +* INFORMATION: Given an id set that value to true ***********************************************************************/ void flags_Set_ID(int id); /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Set flag to false(0) +* INFORMATION: Given an id set that value to false ***********************************************************************/ void flags_Clear_ID(int id); /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Toggle a flag +* INFORMATION: Given an id changes the current value of a flag to its +* opposite. ***********************************************************************/ void flags_Toggle_ID(int id); /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Checks if a flag is set +* INFORMATION: Given an id value, check if that is set to true in the +* bitfield ***********************************************************************/ bool flags_IsSet_ID(int id); /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Set flag to true(1) +* INFORMATION: Given a mask set that value to true ***********************************************************************/ void flags_Set_MASK(int mask); /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Set flag to false(0) +* INFORMATION: Given a mask set that value to false ***********************************************************************/ void flags_Clear_MASK(int mask); /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Toggle a flag +* INFORMATION: Given a mask changes the current value of a flag to its +* opposite. ***********************************************************************/ void flags_Toggle_MASK(int mask); /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Checks if a flag is set +* INFORMATION: Given a mask value, check if that is set to true in the +* bitfield ***********************************************************************/ bool flags_IsSet_MASK(int mask); diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h index 498bb8d..066a92a 100644 --- a/UAV-ControlSystem/inc/drivers/motormix.h +++ b/UAV-ControlSystem/inc/drivers/motormix.h @@ -25,7 +25,7 @@ #define MOTOR_COUNT 10 // TODO: These are only temporary before merge with PID part -uint16_t PID_Out[3]; +int16_t PID_Out[3]; // TODO: Temporary before PID typedef enum { diff --git a/UAV-ControlSystem/inc/drivers/motors.h b/UAV-ControlSystem/inc/drivers/motors.h index ac89685..de79654 100644 --- a/UAV-ControlSystem/inc/drivers/motors.h +++ b/UAV-ControlSystem/inc/drivers/motors.h @@ -8,6 +8,8 @@ #ifndef DRIVERS_MOTORS_H_ #define DRIVERS_MOTORS_H_ +#define MAX_PULSE 1950 + #include "stm32f4xx_revo.h" /* Struct of motor output protocol */ typedef enum { @@ -15,6 +17,8 @@ typedef enum { Oneshot125 }motorOutput; +extern bool perfromMotorCalibration; + /************************************************************************** * BRIEF: Initializing a motor (maps a pwm signal to a certain pin) * * INFORMATION: The motor becomes active, Each motor configuration is changed in the "stm32f4xx_revo.h" file * @@ -70,5 +74,13 @@ void pwmAdjustSpeedOfMotor(uint8_t motor, uint16_t pulse); **************************************************************************/ void pwmAdjustSpeedOfMotorDutyCycle(uint8_t motor, uint16_t DutyCycle); +/************************************************************************** +* BRIEF: Calibrates the motors if it should * +* INFORMATION: Will perform a sequence that will calibrate the motors. The +* motors will only be calibrated if a boolean value that is ´ +* checked inside have been set to true * +**************************************************************************/ +void calibrateMotors(); + #endif /* DRIVERS_MOTORS_H_ */ diff --git a/UAV-ControlSystem/inc/drivers/sbus.h b/UAV-ControlSystem/inc/drivers/sbus.h index 8076b61..486268a 100644 --- a/UAV-ControlSystem/inc/drivers/sbus.h +++ b/UAV-ControlSystem/inc/drivers/sbus.h @@ -25,7 +25,7 @@ #define SBUS_FRAME_SIZE 25 #define SBUS_HEADER 0x0F #define SBUS_FOOTER 0x00 -#define USART1_SBUS_DMA_SIZE SBUS_FRAME_SIZE + 1 // sbus package is 176 bits (22 bytes) +#define USART1_SBUS_DMA_SIZE SBUS_FRAME_SIZE + 1 // sbus package is 176 bits (22 bytes) #define SBUS_MAX_CHANNEL 18 // Including two digital #define STICK_CHANNEL_COUNT 4 diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index afca423..1d71484 100644 --- a/UAV-ControlSystem/inc/stm32f4xx_revo.h +++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h @@ -196,15 +196,15 @@ #define MOTOR_6_CHANNEL TIM_CHANNEL_1 #define MOTOR_7 7 -#define MOTOR_7_PIN GPIO_PIN_6 -#define MOTOR_7_PORT GPIOC -#define MOTOR_7_TIM TIM8 +#define MOTOR_7_PIN GPIO_PIN_14 +#define MOTOR_7_PORT GPIOB +#define MOTOR_7_TIM TIM12 #define MOTOR_7_CHANNEL TIM_CHANNEL_1 #define MOTOR_8 8 -#define MOTOR_8_PIN GPIO_PIN_7 -#define MOTOR_8_PORT GPIOC -#define MOTOR_8_TIM TIM8 +#define MOTOR_8_PIN GPIO_PIN_15 +#define MOTOR_8_PORT GPIOB +#define MOTOR_8_TIM TIM12 #define MOTOR_8_CHANNEL TIM_CHANNEL_2 #define MOTOR_9 9 diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h index 0cc221e..607b8e1 100644 --- a/UAV-ControlSystem/inc/system_variables.h +++ b/UAV-ControlSystem/inc/system_variables.h @@ -14,7 +14,7 @@ #ifndef SYSTEM_VARIABLES_H_ #define SYSTEM_VARIABLES_H_ -#define EEPROM_SYS_VERSION 101 +#define EEPROM_SYS_VERSION 102 #define ADC_STATE #include "stm32f4xx.h" diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index b6338fa..dcf7d2f 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -23,6 +23,7 @@ #include #include "utilities.h" #include "Scheduler/scheduler.h" +#include "drivers/motors.h" #define cliActivateCharacter 35 #define commandValueError 0xFFFFFFFFFFFFFFFF @@ -71,6 +72,7 @@ typedef enum ACTION_REBOOT, //Reboots the entire system ACTION_RESET, //Resets the entire eeprom ACTION_STATS, //gives statistics on the system + ACTION_MOTORCALIBRATE, //The number of actions ACTION_COUNT, //Count of the number of actions @@ -91,7 +93,8 @@ const typeString commandAction_Strings[ACTION_COUNT] = { "exit", "reboot", "reset", - "stats" + "stats", + "motorcalibrate" }; /* String values descrbing information of a certain action */ @@ -108,6 +111,7 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = { "| reboot - Exit CLI and reboots the system.\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" + "| motorcalibrate - Calibrates all motors." }; /* String array containing all the signature examples for each action */ @@ -123,7 +127,8 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = { " exit", " reboot", " reset", - " stats" + " stats", + " motorcalibrate" }; /* Size values for the values a command will require */ @@ -148,6 +153,9 @@ typedef enum COMMAND_ID_ADC_SCALES_RIGHT, COMMAND_ID_PID_ROLL_KP, + /* calibrate motors command */ + COMMAND_ID_MOTORCALIBRATE, + /* Period values for tasks */ COMMAND_ID_PERIOD_SYSTEM , COMMAND_ID_PERIOD_GYROPID, @@ -209,13 +217,13 @@ typedef enum COMMAND_ID_FLAG_FLIGHTMODE_GPS_MAXRANGE, COMMAND_ID_FLAG_FLIGHTMODE_GPS_CHANNEL, - COMMAND_ID_FLAG_AIRMODE_MINRANGE, - COMMAND_ID_FLAG_AIRMODE_MAXRANGE, - COMMAND_ID_FLAG_AIRMODE_CHANNEL, + COMMAND_ID_FLAG_MIXERFULLSCALE_MINRANGE, + COMMAND_ID_FLAG_MIXERFULLSCALE_MAXRANGE, + COMMAND_ID_FLAG_MIXERFULLSCALE_CHANNEL, - COMMAND_ID_FLAG_FLIGHTMODE_2_MINRANGE, - COMMAND_ID_FLAG_FLIGHTMODE_2_MAXRANGE, - COMMAND_ID_FLAG_FLIGHTMODE_2_CHANNEL, + COMMAND_ID_FLAG_MIXERLOWSCALE_MINRANGE, + COMMAND_ID_FLAG_MIXERLOWSCALE_MAXRANGE, + COMMAND_ID_FLAG_MIXERLOWSCALE_CHANNEL, COMMAND_ID_FLAG_FLIGHTMODE_3_MINRANGE, COMMAND_ID_FLAG_FLIGHTMODE_3_MAXRANGE, @@ -269,6 +277,16 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { { "pid_roll_kp", COMMAND_ID_PID_ROLL_KP, EEPROM_PID_ROLL_KP, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} }, + + + /* Calibrate motors */ + [COMMAND_ID_MOTORCALIBRATE] = + { + "motor_calibration", COMMAND_ID_MOTORCALIBRATE, EEPROM_MOTORCALIBRATE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_BOOL, .valueRange = {0, 1} + }, + + + /* set period commands */ [COMMAND_ID_PERIOD_SYSTEM] = { "task_period_system", COMMAND_ID_PERIOD_SYSTEM, EEPROM_PERIOD_SYSTEM, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_32, .valueRange = {0, 1000000000} @@ -440,30 +458,30 @@ 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_AIRMODE_MINRANGE] = + [COMMAND_ID_FLAG_MIXERFULLSCALE_MINRANGE] = { - "flag_airmode_minrange", COMMAND_ID_FLAG_AIRMODE_MINRANGE, EEPROM_FLAG_AIRMODE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_16, .valueRange = {0, 2500} + "flag_mixerfullscale_minrange", COMMAND_ID_FLAG_MIXERFULLSCALE_MINRANGE, EEPROM_FLAG_MIXERFULLSCALE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_16, .valueRange = {0, 2500} }, - [COMMAND_ID_FLAG_AIRMODE_MAXRANGE] = + [COMMAND_ID_FLAG_MIXERFULLSCALE_MAXRANGE] = { - "flag_airmode_maxrange", COMMAND_ID_FLAG_AIRMODE_MAXRANGE, EEPROM_FLAG_AIRMODE, EEPROM_VALUE_TYPE_SYSTEM, 2, VAL_UINT_16, .valueRange = {0, 2500} + "flag_mixerfullscale_maxrange", COMMAND_ID_FLAG_MIXERFULLSCALE_MAXRANGE, EEPROM_FLAG_MIXERFULLSCALE, EEPROM_VALUE_TYPE_SYSTEM, 2, VAL_UINT_16, .valueRange = {0, 2500} }, - [COMMAND_ID_FLAG_AIRMODE_CHANNEL] = + [COMMAND_ID_FLAG_MIXERFULLSCALE_CHANNEL] = { - "flag_airmode_channel", COMMAND_ID_FLAG_AIRMODE_CHANNEL, EEPROM_FLAG_AIRMODE, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18} + "flag_mixerfullscale_channel", COMMAND_ID_FLAG_MIXERFULLSCALE_CHANNEL, EEPROM_FLAG_MIXERFULLSCALE, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18} }, - [COMMAND_ID_FLAG_FLIGHTMODE_2_MINRANGE] = + [COMMAND_ID_FLAG_MIXERLOWSCALE_MINRANGE] = { - "flag_flightmode_2_minrange", COMMAND_ID_FLAG_FLIGHTMODE_2_MINRANGE, EEPROM_FLAG_FLIGHTMODE_2, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_16, .valueRange = {0, 2500} + "flag_mixerlowscale_minrange", COMMAND_ID_FLAG_MIXERLOWSCALE_MINRANGE, EEPROM_FLAG_MIXERLOWSCALE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_UINT_16, .valueRange = {0, 2500} }, - [COMMAND_ID_FLAG_FLIGHTMODE_2_MAXRANGE] = + [COMMAND_ID_FLAG_MIXERLOWSCALE_MAXRANGE] = { - "flag_flightmode_2_maxrange", COMMAND_ID_FLAG_FLIGHTMODE_2_MAXRANGE, EEPROM_FLAG_FLIGHTMODE_2, EEPROM_VALUE_TYPE_SYSTEM, 2, VAL_UINT_16, .valueRange = {0, 2500} + "flag_mixerlowscale_maxrange", COMMAND_ID_FLAG_MIXERLOWSCALE_MAXRANGE, EEPROM_FLAG_MIXERLOWSCALE, EEPROM_VALUE_TYPE_SYSTEM, 2, VAL_UINT_16, .valueRange = {0, 2500} }, - [COMMAND_ID_FLAG_FLIGHTMODE_2_CHANNEL] = + [COMMAND_ID_FLAG_MIXERLOWSCALE_CHANNEL] = { - "flag_flightmode_2_channel", COMMAND_ID_FLAG_FLIGHTMODE_2_CHANNEL, EEPROM_FLAG_FLIGHTMODE_2, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18} + "flag_mixerlowscale_channel", COMMAND_ID_FLAG_MIXERLOWSCALE_CHANNEL, EEPROM_FLAG_MIXERLOWSCALE, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18} }, [COMMAND_ID_FLAG_FLIGHTMODE_3_MINRANGE] = @@ -480,8 +498,6 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { }, - - }; /*********************************************************************** @@ -1259,6 +1275,22 @@ void cliRun() case commandMask(commandSize_1, ACTION_STATS): TransmitSystemStats(); break; + case commandMask(commandSize_1, ACTION_MOTORCALIBRATE): + TransmitBack("- Make sure PROPELLERS are REMOVED and be prepared that the aircraft my start the engines\n\n\r!"); + TransmitBack("- REMOVE HANDS FROM THE AIRCRAFT\n\n\r"); + + TransmitBack("Do you really want to calibrate? (y/n)\n\n\r"); + if (ReceiveBinaryDecision(121,110)) + { + TransmitBack("Starting calibration, BE CAREFUL!!! \n\n\r"); + calibrateMotors(); + } + else + { + TransmitBack("Exiting the calibration, BE CAREFUL!!! \n\n\r"); + } + + 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 15e85c1..45e73df 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -23,6 +23,7 @@ #include "Scheduler/scheduler.h" #include "drivers/failsafe_toggles.h" #include "drivers/motormix.h" +#include "drivers/motors.h" /* Reads the EEPROM version from EEPROM - Is compared to EEPROM_SYS_VERSION */ uint8_t stored_eeprom_identifier; @@ -95,6 +96,14 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = { .dataPtr = &uart1_rx_inverter, }, + /* Motor calibrate */ + [EEPROM_MOTORCALIBRATE] = + { + .size = sizeof(bool), + .dataPtr = &perfromMotorCalibration, + }, + + /* Task eeprom values */ [EEPROM_PERIOD_SYSTEM] = { @@ -222,15 +231,15 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = { .size = sizeof(flags_Configuration_t), .dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_GPS]), }, - [EEPROM_FLAG_AIRMODE] = + [EEPROM_FLAG_MIXERFULLSCALE] = { .size = sizeof(flags_Configuration_t), - .dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_AIRMODE]), + .dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_MIXERFULLSCALE]), }, - [EEPROM_FLAG_FLIGHTMODE_2] = + [EEPROM_FLAG_MIXERLOWSCALE] = { .size = sizeof(flags_Configuration_t), - .dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_2]), + .dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_MIXERLOWSCALE]), }, [EEPROM_FLAG_FLIGHTMODE_3] = { diff --git a/UAV-ControlSystem/src/drivers/failsafe_toggles.c b/UAV-ControlSystem/src/drivers/failsafe_toggles.c index d9056f5..3101c34 100644 --- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c +++ b/UAV-ControlSystem/src/drivers/failsafe_toggles.c @@ -1,11 +1,26 @@ -/* - * failsafe_toggles.c - * - * Created on: 10 okt. 2016 - * Author: holmis - */ + /********************************************************************** + * NAME: failsafe_toggles.c * + * AUTHOR: Jonas Holmberg * + * PURPOSE: Give the ability to create boolean flags and failsafe * + * variables. * + * INFORMATION: Create any boolean variable that should be able to be * + * used throughout the system. These can be boolean vales * + * for standard checks, failsafes etc. These can also be * + * linked to a RC channel and value range wherein the * + * value will be set to one. * + * * + * GLOBAL VARIABLES: * + * Variable Type Description * + * -------- ---- ----------- * + * systemFlags boolFlags_t Represents a bitfield of all the * + * boolean values in the system. * + * flagConfigArr flags_Configuration_t Configuration values for any * + * flag that should be changeable * + * from the RC controller. * + **********************************************************************/ #include "drivers/failsafe_toggles.h" +#include "drivers/sbus.h" /* Stuct containing values for all the flags and failsafe booleans sin the system */ boolFlags_t systemFlags = {{0}}; @@ -42,17 +57,17 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = { .channelNumber = 0, .flagId = systemFlags_flightmode_gps_id, }, - [FLAG_CONFIGURATION_AIRMODE] = { + [FLAG_CONFIGURATION_MIXERFULLSCALE] = { .minRange = 0, .maxRange = 0, .channelNumber = 0, - .flagId = systemFlags_airmode_id, + .flagId = systemFlags_mixerfullscale_id, }, - [FLAG_CONFIGURATION_FLIGHTMODE_2] = { + [FLAG_CONFIGURATION_MIXERLOWSCALE] = { .minRange = 0, .maxRange = 0, .channelNumber = 0, - .flagId = systemFlags_flightMode_2_id, + .flagId = systemFlags_mixerlowscale_id, }, [FLAG_CONFIGURATION_FLIGHTMODE_3] = { .minRange = 0, @@ -63,17 +78,19 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = { }; /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Could be used to set start values for some values +* INFORMATION: Possible to set the values for any of the failsafes ***********************************************************************/ void initFlags() { - systemFlags.intRepresentation = 0; + //Could be used to set start values for failsafes. } /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Process RC channels that should operate on some flags +* INFORMATION: Reads the value of the RC channels that and checks if +* any of the channels should handle some flag value in the +* system. If it should it will update its state. ***********************************************************************/ void flags_ProcessRcChannel(int rcChannel_ID, int value) { @@ -94,8 +111,42 @@ void flags_ProcessRcChannel(int rcChannel_ID, int value) } /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Process RC channels that should operate on some flags +* INFORMATION: Reads the value of the RC channels that and checks if +* any of the channels should handle some flag value in the +* system. If it should it will update its state. +***********************************************************************/ +void flags_ProcessRcChannel_Improved(uint8_t minChannel, uint8_t maxChannel) +{ + //Loop through all the active flag configuration values + for (int i = 0; i < FLAG_CONFIGURATION_COUNT; i++) + { + int currentChannelNumb = flagConfigArr[i].channelNumber; + //Check if the current Flag channel is within the set bounds + if(currentChannelNumb >= 5 && currentChannelNumb <= maxChannel) + { + //Get the value for the channel that the current flag is linked to + int value = getChannelValue(sbusChannelData, currentChannelNumb); + + if(value >= flagConfigArr[i].minRange && value <= flagConfigArr[i].maxRange) + { + flags_Set_ID(flagConfigArr[i].flagId); + } + else + { + flags_Clear_ID(flagConfigArr[i].flagId); + } + } + else + { + continue; + } + } +} + +/*********************************************************************** +* BRIEF: Set flag to true(1) +* INFORMATION: Given a mask set that value to true ***********************************************************************/ void flags_Set_MASK(int mask) { @@ -103,8 +154,8 @@ void flags_Set_MASK(int mask) } /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Set flag to false(0) +* INFORMATION: Given a mask set that value to false ***********************************************************************/ void flags_Clear_MASK(int mask) { @@ -112,8 +163,9 @@ void flags_Clear_MASK(int mask) } /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Toggle a flag +* INFORMATION: Given a mask changes the current value of a flag to its +* opposite. ***********************************************************************/ void flags_Toggle_MASK(int mask) { @@ -121,8 +173,9 @@ void flags_Toggle_MASK(int mask) } /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Checks if a flag is set +* INFORMATION: Given a mask value, check if that is set to true in the +* bitfield ***********************************************************************/ bool flags_IsSet_MASK(int mask) { @@ -130,8 +183,8 @@ bool flags_IsSet_MASK(int mask) } /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Set flag to true(1) +* INFORMATION: Given an id set that value to true ***********************************************************************/ void flags_Set_ID(int id) { @@ -139,8 +192,8 @@ void flags_Set_ID(int id) } /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Set flag to false(0) +* INFORMATION: Given an id set that value to false ***********************************************************************/ void flags_Clear_ID(int id) { @@ -148,8 +201,9 @@ void flags_Clear_ID(int id) } /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Toggle a flag +* INFORMATION: Given an id changes the current value of a flag to its +* opposite. ***********************************************************************/ void flags_Toggle_ID(int id) { @@ -157,8 +211,9 @@ void flags_Toggle_ID(int id) } /*********************************************************************** -* BRIEF: -* INFORMATION: +* BRIEF: Checks if a flag is set +* INFORMATION: Given an id value, check if that is set to true in the +* bitfield ***********************************************************************/ bool flags_IsSet_ID(int id) { diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 433dcca..1eceb28 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -45,9 +45,9 @@ uint16_t motor_output[MOTOR_COUNT]; // TODO: Implement in EEPROM mixerConfig_s mixerConfig = { .minThrottle = 1050, - .maxThrottle = 1950, + .maxThrottle = MAX_PULSE - 100, .minCommand = 1000, - .maxCommand = 2000, + .maxCommand = MAX_PULSE, .minCheck = 1010, .pid_at_min_throttle = true, .motorstop = false, @@ -97,14 +97,15 @@ void mix() //uint16_t throttleIdle = mixerConfig.minThrottle + (throttleRange / 2); int16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array - uint16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor - uint16_t RPY_Mix_Max = 0; // Maximum desired command for any motor - uint16_t throttle = sbusChannelData.chan3; // Import throttle value from remote + int16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor + int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor + int16_t throttle = sbusChannelData.chan3; // Import throttle value from remote // Might be used for some debug if necessary static bool motorLimitReached; - if (flags_IsSet_ID(systemFlags_airmode_id)) // TODO: replace with check for Airmode + /* Mixer Full Scale enabled */ + if (flags_IsSet_ID(systemFlags_mixerfullscale_id)) { for (int i = 0; i < MOTOR_COUNT; i++) { @@ -122,14 +123,16 @@ void mix() } - uint16_t throttleRange = mixerConfig.maxCommand - mixerConfig.minThrottle; // The throttle range we have with current defines - uint16_t RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min; // Range of the desired mixer outputs - uint16_t throttleMin = mixerConfig.minThrottle; // Import system variable - uint16_t throttleMax = mixerConfig.maxThrottle; // Import + int16_t RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min; // Range of the desired mixer outputs + int16_t throttleMin = mixerConfig.minThrottle; // Import system variable + int16_t throttleMax = mixerConfig.maxCommand; // Import + int16_t throttleRange = throttleMax - throttleMin; // The throttle range we have with current defines + int16_t throttleMid = (throttleMax + throttleMin) / 2; /* Check if we have enough interval for the adjustments */ - // Check if we maxed out + + /* Check if we maxed out */ if (RPY_MixRange > throttleRange) { motorLimitReached = true; // Yes, we maxed out @@ -139,7 +142,12 @@ void mix() // Apply the scaling to all outputs for (int i = 0; i < MOTOR_COUNT; i++) RPY_Mix[i] = RPY_Mix[i] * mixReduction; + + //Temp fix may not be right - Set throttle to exakt half (As this craft is symmetric) + throttle = mixerConfig.minThrottle + mixReduction * RPY_Mix_Max; + } + // If we have the needed range no scaling is needed else { @@ -147,22 +155,37 @@ void mix() /* Update min and max throttle so we can add the * calculated adjustments and still just max out */ - throttleMin += (RPY_MixRange / 2); - throttleMax -= (RPY_MixRange / 2); - } + throttleMin += (RPY_MixRange / 2); // Can be removed. Just made to be sure + throttleMax -= (RPY_MixRange / 2); // Can be removed. Just made to be sure + /* The scaling of the throttle in the room thats left from the mixer */ + float throttleRescale = ((float)(throttleRange - RPY_MixRange)) / (float)throttleRange; + + /* Make sure throttle mostly is inside range. Mostly above minthrottle before scaling */ + throttle = constrain(throttle, mixerConfig.minThrottle, mixerConfig.maxCommand); + + /* Converting throttle to value centered around 0 */ + throttle = (throttle - (mixerConfig.maxCommand - throttleRange / 2)); + + /* Rescaling throttle */ + throttle = throttle*throttleRescale; + + /* Adding new scaled throttle to throttleMid */ + throttle = throttleMid + throttle; + + } // Now we add desired throttle for (int i = 0; i < MOTOR_COUNT; i++) - { - // Constrain in within the regulation of the mix - motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, mixerConfig.maxCommand); - motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand); - } + // Constrain in within the regulation of the mix - OBS. Constrain can be removed. Just to make sure + motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax); } - else // Airmode not active + else // Mixer full scale NOT active { - int16_t maxMotor = 0; + uint16_t throttleMid = (mixerConfig.minCommand + mixerConfig.maxThrottle) / 2; + /* This time we need to check against mid throttle */ + RPY_Mix_Min = throttleMid; + RPY_Mix_Max = throttleMid; for (int i = 0; i < MOTOR_COUNT; i++) // Without airmode this includes throttle { @@ -172,15 +195,37 @@ void mix() PID_Out[ROLL] * mixerUAV[i].roll + \ PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); - // Find the maximum motor output - if (RPY_Mix[i] > maxMotor) maxMotor = RPY_Mix[i]; + // Find the minimum and maximum motor output + if (RPY_Mix[i] > RPY_Mix_Max) RPY_Mix_Max = RPY_Mix[i]; + if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i]; } - int16_t maxThrottleDifference = 0; - if (maxMotor > mixerConfig.maxThrottle) - maxThrottleDifference = maxMotor - mixerConfig.maxThrottle; - // Approach at mixing + /* Mixer Low Scale - Scaling output around low throttle */ + if (flags_IsSet_ID(systemFlags_mixerlowscale_id)) + { + uint16_t RPY_Mix_Min_Overshoot = (RPY_Mix_Min < mixerConfig.minThrottle) ? mixerConfig.minThrottle - RPY_Mix_Min : 0; + + if (RPY_Mix_Min_Overshoot > 0 ) //|| RPY_Mix_Max_Overshoot > 0) + { + /* How far away are we from throttle */ + uint16_t RPY_Mix_Thr_Displacement = throttle - RPY_Mix_Min; + + /* Scaling factor */ + float mix_scale_reduction = ((float)RPY_Mix_Min_Overshoot / (float)RPY_Mix_Thr_Displacement); + + /* Rescaling */ + for (int i = 0; i < MOTOR_COUNT; i++) + RPY_Mix[i] = (((float)(RPY_Mix[i] - throttle)) * (1 - mix_scale_reduction)) + throttle; + } + } + + /* Check how far over maxCommand we are (overflow) */ + int16_t maxThrottleDifference = 0; + if (RPY_Mix_Max > mixerConfig.maxCommand) + maxThrottleDifference = RPY_Mix_Max - mixerConfig.maxCommand; + + // Last flag checks and output reduction for (int i = 0; i < MOTOR_COUNT; i++) { RPY_Mix[i] -= maxThrottleDifference; // We reduce the highest overflow on all motors @@ -200,7 +245,7 @@ void mix() // Though authority is very low with low throttle } - // Constrain in within the valid motor outputs + /* Constrain in within the valid motor outputs */ motor_output[i] = constrain(RPY_Mix[i], mixerConfig.minCommand, mixerConfig.maxCommand); } } @@ -209,7 +254,7 @@ void mix() for (int i = 0; i < MOTOR_COUNT; i++) { /* If engines are armed then give the output to the motors */ - if (flags_IsSet_ID(systemFlags_armed_id)) // TODO: replace with check for armed (IF ARMED) + if (flags_IsSet_ID(systemFlags_armed_id)) motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand); /* If not then stop motors */ else diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c index 75a85b3..b2a83d9 100644 --- a/UAV-ControlSystem/src/drivers/motors.c +++ b/UAV-ControlSystem/src/drivers/motors.c @@ -12,6 +12,8 @@ #include "stm32f4xx_revo.h" #include "drivers/pwm.h" #include "drivers/motors.h" +#include "drivers/failsafe_toggles.h" +#include "config/eeprom.h" const int MotorPWMPeriode = 2000; //Micro seconds const int MotorPWMInitPulse = 1000; @@ -25,6 +27,8 @@ typedef struct uint32_t channel; //TIM_CHANNEL_1/TIM_CHANNEL_1/.. }motorProfile; +bool perfromMotorCalibration = false; + /************************************************************************** * BRIEF: Returns the final pulse of a motor driver call * INFORMATION: The pulse is not allowed to be higher then 94 % of the total periode of the pwm signal, otherwise the pwm won't perform correctly @@ -32,7 +36,7 @@ typedef struct **************************************************************************/ uint16_t checkPulse(uint16_t pulse) { - return ((pulse/MotorPWMPeriode)*100 < 98)? pulse: MotorPWMPeriode*0.98; + return (pulse > MAX_PULSE) ? MAX_PULSE : pulse; } /************************************************************************** @@ -228,3 +232,32 @@ void pwmAdjustSpeedOfMotorDutyCycle(uint8_t motor, uint16_t DutyCycle) setPwmPulse(profile.channel, profile.tim, checkPulse(pulse)); } + +/************************************************************************** +* BRIEF: Calibrates the motors if it should * +* INFORMATION: Will perform a sequence that will calibrate the motors. The +* motors will only be calibrated if a boolean value that is ´ +* checked inside have been set to true. This value will be ´ +* set back to false after the function have been compleeted * +**************************************************************************/ +void calibrateMotors() +{ + + pwmDeactivateAllMotors(); + HAL_Delay(1500); + pwmActivateAllMotors(); + + //First set the motor output to the maximum allowed throttle + for (uint8_t i = 1; i < 11; i++ ) pwmAdjustSpeedOfMotor(i,MotorPWMPeriode); + + //wait for a little while + HAL_Delay(6200); + + //Now set the speed of all motors to the lowest + for (uint8_t i = 1; i < 11; i++ ) pwmAdjustSpeedOfMotor(i,MotorPWMInitPulse); + + + perfromMotorCalibration = false; + saveEEPROM(); + +} diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c index e518afd..8df2742 100644 --- a/UAV-ControlSystem/src/drivers/sbus.c +++ b/UAV-ControlSystem/src/drivers/sbus.c @@ -28,7 +28,7 @@ /* This instance is read by the whole system and should contain actual RX data */ sbusFrame_s sbusChannelData = {0}; -dma_usart_return raw_dma_data_t = {0}; +dma_usart_return raw_dma_data_t; /* Create a DMA Handler */ usart_dma_profile dmaHandler; @@ -102,69 +102,132 @@ void sbus_read() { // Holds what we've read so far static uint8_t sbus_arr[SBUS_FRAME_SIZE]; + static uint8_t message_it = 0; + static uint32_t missedMsg = 0; + static uint8_t message_it_secondary_head = 0; + static bool new_header = false; static int sbus_arr_iterator = SBUS_FRAME_SIZE; static bool stop_bit_read = false; + // If continue only if we get new data from DMA if (raw_dma_data_t.new_data) { + + + for (int i = 0; i < USART1_SBUS_DMA_SIZE; i++) { + uint8_t msg = raw_dma_data_t.buff[i]; // Look for the beginning of a sbus frame - if (raw_dma_data_t.buff[i] == (uint8_t)SBUS_HEADER && stop_bit_read) + if ( message_it == 0 ) //&& stop_bit_read) { - sbus_arr_iterator = 0; - stop_bit_read = false; + //message_it = (raw_dma_data_t.buff[i] == ((uint8_t)SBUS_HEADER)) ? 1 : 0; + if (msg == ((uint8_t)SBUS_HEADER)) + { + sbus_arr[(message_it)] = msg; + message_it++; + new_header = false; + } + else + { + message_it = 0; + } + +// sbus_arr_iterator = 0; +// stop_bit_read = false; } // Look for the end of sbus frame - else if(raw_dma_data_t.buff[i] == (uint8_t)SBUS_FOOTER) + //else if(raw_dma_data_t.buff[i] == (uint8_t)SBUS_FOOTER) + else { - stop_bit_read = true; - // If the expected byte is stop byte, then we overwrite to the return value. - if (sbus_arr_iterator == SBUS_FRAME_SIZE - 1) + if (msg == (uint8_t)SBUS_HEADER && new_header == false) { - sbusChannelData = *(sbusFrame_s*)sbus_arr; + new_header = true; + message_it_secondary_head = message_it; //save the value of the position in The buffer array, not the dma array index + } - // Linear fitting + if ((message_it) < SBUS_FRAME_SIZE) + { + sbus_arr[(message_it)] = msg; + message_it++; + } - sbusChannelData.chan1 = SBUS_UNIT_CONV(sbusChannelData.chan1); - sbusChannelData.chan2 = SBUS_UNIT_CONV(sbusChannelData.chan2); - sbusChannelData.chan3 = SBUS_UNIT_CONV(sbusChannelData.chan3); - sbusChannelData.chan4 = SBUS_UNIT_CONV(sbusChannelData.chan4); - sbusChannelData.chan5 = SBUS_UNIT_CONV(sbusChannelData.chan5); - sbusChannelData.chan6 = SBUS_UNIT_CONV(sbusChannelData.chan6); - sbusChannelData.chan7 = SBUS_UNIT_CONV(sbusChannelData.chan7); - sbusChannelData.chan8 = SBUS_UNIT_CONV(sbusChannelData.chan8); + if ((message_it) == SBUS_FRAME_SIZE) + { + missedMsg++; + if (msg == (uint8_t)SBUS_FOOTER) + { + message_it = 0; + missedMsg--; + //stop_bit_read = true; + // If the expected byte is stop byte, then we overwrite to the return value. + //if (sbus_arr_iterator == SBUS_FRAME_SIZE - 1) + //{ + sbusChannelData = *(sbusFrame_s*)sbus_arr; - // TODO: Depending on defines don't process more than necessary - sbusChannelData.chan9 = SBUS_UNIT_CONV(sbusChannelData.chan9); - sbusChannelData.chan10 = SBUS_UNIT_CONV(sbusChannelData.chan10); - sbusChannelData.chan11 = SBUS_UNIT_CONV(sbusChannelData.chan11); - sbusChannelData.chan12 = SBUS_UNIT_CONV(sbusChannelData.chan12); - sbusChannelData.chan13 = SBUS_UNIT_CONV(sbusChannelData.chan13); - sbusChannelData.chan14 = SBUS_UNIT_CONV(sbusChannelData.chan14); - sbusChannelData.chan15 = SBUS_UNIT_CONV(sbusChannelData.chan15); - sbusChannelData.chan16 = SBUS_UNIT_CONV(sbusChannelData.chan16); + // Linear fitting + sbusChannelData.chan1 = SBUS_UNIT_CONV(sbusChannelData.chan1); + sbusChannelData.chan2 = SBUS_UNIT_CONV(sbusChannelData.chan2); + sbusChannelData.chan3 = SBUS_UNIT_CONV(sbusChannelData.chan3); + sbusChannelData.chan4 = SBUS_UNIT_CONV(sbusChannelData.chan4); + sbusChannelData.chan5 = SBUS_UNIT_CONV(sbusChannelData.chan5); + sbusChannelData.chan6 = SBUS_UNIT_CONV(sbusChannelData.chan6); + sbusChannelData.chan7 = SBUS_UNIT_CONV(sbusChannelData.chan7); + sbusChannelData.chan8 = SBUS_UNIT_CONV(sbusChannelData.chan8); - // TODO: Failsafe using defines checking if channels are in range BEFORE we truncate + // TODO: Depending on defines don't process more than necessary + sbusChannelData.chan9 = SBUS_UNIT_CONV(sbusChannelData.chan9); + sbusChannelData.chan10 = SBUS_UNIT_CONV(sbusChannelData.chan10); + sbusChannelData.chan11 = SBUS_UNIT_CONV(sbusChannelData.chan11); + sbusChannelData.chan12 = SBUS_UNIT_CONV(sbusChannelData.chan12); + sbusChannelData.chan13 = SBUS_UNIT_CONV(sbusChannelData.chan13); + sbusChannelData.chan14 = SBUS_UNIT_CONV(sbusChannelData.chan14); + sbusChannelData.chan15 = SBUS_UNIT_CONV(sbusChannelData.chan15); + sbusChannelData.chan16 = SBUS_UNIT_CONV(sbusChannelData.chan16); + // TODO: Failsafe using defines checking if channels are in range BEFORE we truncate + sbusChannelData.chan1 = rx_truncate(sbusChannelData.chan1); + sbusChannelData.chan2 = rx_truncate(sbusChannelData.chan2); + sbusChannelData.chan3 = rx_truncate(sbusChannelData.chan3); + sbusChannelData.chan4 = rx_truncate(sbusChannelData.chan4); + sbusChannelData.chan5 = rx_truncate(sbusChannelData.chan5); + sbusChannelData.chan6 = rx_truncate(sbusChannelData.chan6); + sbusChannelData.chan7 = rx_truncate(sbusChannelData.chan7); + sbusChannelData.chan8 = rx_truncate(sbusChannelData.chan8); + } + else + { + int temp_secondaryHeader = message_it_secondary_head; + message_it = message_it - temp_secondaryHeader; //update the counter to the empty part of the updated array + new_header = false; //set new header to false, this is true if there is another header within the buffer + + //Move all the remaning messages in the buffer to the start of the buffer + for (int i = temp_secondaryHeader; i < SBUS_FRAME_SIZE; i++) + { + int innerCount = i-temp_secondaryHeader; + sbus_arr[innerCount] = sbus_arr[i]; + + //check if we find another possible header inside the rest of the buffer and save that + if (sbus_arr[innerCount] == (uint8_t)SBUS_HEADER && innerCount > 0 && new_header == false ) + { + new_header = true; + message_it_secondary_head = innerCount; + } + } + + } - sbusChannelData.chan1 = rx_truncate(sbusChannelData.chan1); - sbusChannelData.chan2 = rx_truncate(sbusChannelData.chan2); - sbusChannelData.chan3 = rx_truncate(sbusChannelData.chan3); - sbusChannelData.chan4 = rx_truncate(sbusChannelData.chan4); - sbusChannelData.chan5 = rx_truncate(sbusChannelData.chan5); - sbusChannelData.chan6 = rx_truncate(sbusChannelData.chan6); - sbusChannelData.chan7 = rx_truncate(sbusChannelData.chan7); - sbusChannelData.chan8 = rx_truncate(sbusChannelData.chan8); } } - // Copy next byte into the sbus_arr - if (sbus_arr_iterator < SBUS_FRAME_SIZE) - sbus_arr[sbus_arr_iterator] = raw_dma_data_t.buff[i]; - sbus_arr_iterator++; + +// // Copy next byte into the sbus_arr +// if (sbus_arr_iterator < SBUS_FRAME_SIZE) +// sbus_arr[sbus_arr_iterator] = raw_dma_data_t.buff[i]; +// sbus_arr_iterator++; } + } } @@ -235,6 +298,9 @@ int getChannelValue(sbusFrame_s frame, int id) toReturn = frame.flag_DChannel_18; break; #endif + default: + toReturn = 0; + break; } return toReturn; } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 02a0d1d..c91f172 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -29,6 +29,7 @@ #include "drivers/motors.h" #include "Flight/pid.h" + /************************************************************************** * BRIEF: Should contain all the initializations of the system, needs to * run before the scheduler. @@ -49,12 +50,13 @@ void init_system() readEEPROM(); //initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble - cliInit(USART3); + cliInit(USART6); //init sbus, using USART1 sbus_init(); - //init motors to run with oneshot 125 + //init motors to run with oneshot 125, small delay + HAL_Delay(1000); pwmEnableAllMotors(Oneshot125); pidInit(); diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 0019ead..954a858 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -85,10 +85,13 @@ 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, /if not work change i to start at numb of stick channels - { - flags_ProcessRcChannel(i+1, getChannelValue(frame, i+1)); - } +// 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;