philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'refs/remotes/origin/master' into PID

Browse Source
This commit is contained in:
Lennart Eriksson 2016-10-18 12:11:15 +02:00
commit d22c77fc06
16 changed files with 513 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
UAV-ControlSystem/inc/config/cli.h
View File

@ -55,5 +55,50 @@ bool cliHasMessage();
bool cliShouldRun(); bool cliShouldRun();
#endif /* CONFIG_CLI_H_ */ #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.
* */

7
UAV-ControlSystem/inc/config/eeprom.h
View File

@ -147,6 +147,9 @@ typedef enum {
EEPROM_ADC_SCALES, EEPROM_ADC_SCALES,
EEPROM_UART1_RX_INV, EEPROM_UART1_RX_INV,
/* Motor calibrate bool */
EEPROM_MOTORCALIBRATE,
/* Period values for tasks */ /* Period values for tasks */
EEPROM_PERIOD_SYSTEM, EEPROM_PERIOD_SYSTEM,
EEPROM_PERIOD_GYROPID, EEPROM_PERIOD_GYROPID,
@ -184,8 +187,8 @@ typedef enum {
EEPROM_FLAG_FLIGHTMODE_BAROMETER, EEPROM_FLAG_FLIGHTMODE_BAROMETER,
EEPROM_FLAG_FLIGHTMODE_COMPASS, EEPROM_FLAG_FLIGHTMODE_COMPASS,
EEPROM_FLAG_FLIGHTMODE_GPS, EEPROM_FLAG_FLIGHTMODE_GPS,
EEPROM_FLAG_AIRMODE, EEPROM_FLAG_MIXERFULLSCALE,
EEPROM_FLAG_FLIGHTMODE_2, EEPROM_FLAG_MIXERLOWSCALE,
EEPROM_FLAG_FLIGHTMODE_3, EEPROM_FLAG_FLIGHTMODE_3,
/* Counts the amount of system settings */ /* Counts the amount of system settings */

98
UAV-ControlSystem/inc/drivers/failsafe_toggles.h
View File

@ -1,9 +1,23 @@
/* /**********************************************************************
* failsafe_toggles.h * NAME: failsafe_toggles.h *
* * AUTHOR: Jonas Holmberg *
* Created on: 10 okt. 2016 * PURPOSE: Give the ability to create boolean flags and failsafe *
* Author: holmis * 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_ #ifndef DRIVERS_FAILSAFE_TOGGLES_H_
#define 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_barometer_id 2 + boolean_vals_offset
#define systemFlags_flightmode_compass_id 3 + boolean_vals_offset #define systemFlags_flightmode_compass_id 3 + boolean_vals_offset
#define systemFlags_flightmode_gps_id 4 + boolean_vals_offset #define systemFlags_flightmode_gps_id 4 + boolean_vals_offset
#define systemFlags_airmode_id 5 + boolean_vals_offset #define systemFlags_mixerfullscale_id 5 + boolean_vals_offset
#define systemFlags_flightMode_2_id 6 + boolean_vals_offset #define systemFlags_mixerlowscale_id 6 + boolean_vals_offset
#define systemFlags_flightMode_3_id 7 + 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_barometer_mask getFlagMaskValue(systemFlags_flightmode_barometer_id)
#define systemFlags_flightmode_compass_mask getFlagMaskValue(systemFlags_flightmode_compass_id) #define systemFlags_flightmode_compass_mask getFlagMaskValue(systemFlags_flightmode_compass_id)
#define systemFlags_flightmode_gps_mask getFlagMaskValue(systemFlags_flightmode_gps_id) #define systemFlags_flightmode_gps_mask getFlagMaskValue(systemFlags_flightmode_gps_id)
#define systemFlags_airmode_mask getFlagMaskValue(systemFlags_airmode_id) #define systemFlags_mixerfullscale_mask getFlagMaskValue(systemFlags_mixerfullscale_id)
#define systemFlags_flightMode_2_mask getFlagMaskValue(systemFlags_flightMode_2_id) #define systemFlags_mixerlowscale_mask getFlagMaskValue(systemFlags_mixerlowscale_id)
#define systemFlags_flightMode_3_mask getFlagMaskValue(systemFlags_flightMode_3_id) #define systemFlags_flightMode_3_mask getFlagMaskValue(systemFlags_flightMode_3_id)
@ -67,8 +81,8 @@ typedef union bitSetRegion
booleanValue_t barometer : 1; booleanValue_t barometer : 1;
booleanValue_t compass : 1; booleanValue_t compass : 1;
booleanValue_t gps : 1; booleanValue_t gps : 1;
booleanValue_t airmode : 1; booleanValue_t mixerfullscale : 1;
booleanValue_t flightMode_2 : 1; booleanValue_t mixerlowscale : 1;
booleanValue_t flightMode_3 : 1; booleanValue_t flightMode_3 : 1;
}bitField; }bitField;
uint64_t intRepresentation; uint64_t intRepresentation;
@ -90,77 +104,91 @@ typedef enum
FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER, FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER,
FLAG_CONFIGURATION_FLIGHTMODE_COMPASS, FLAG_CONFIGURATION_FLIGHTMODE_COMPASS,
FLAG_CONFIGURATION_FLIGHTMODE_GPS, FLAG_CONFIGURATION_FLIGHTMODE_GPS,
FLAG_CONFIGURATION_AIRMODE, FLAG_CONFIGURATION_MIXERFULLSCALE,
FLAG_CONFIGURATION_FLIGHTMODE_2, FLAG_CONFIGURATION_MIXERLOWSCALE,
FLAG_CONFIGURATION_FLIGHTMODE_3, FLAG_CONFIGURATION_FLIGHTMODE_3,
//Counter //Counter
FLAG_CONFIGURATION_COUNT FLAG_CONFIGURATION_COUNT
}flags_configuration_IDs_t; }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; extern boolFlags_t systemFlags;
/* Array of flag configurations. These are values that can be set by RC. */ /* Array of flag configurations. These are values that can be set by RC. */
extern flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT]; extern flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT];
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Could be used to set start values for some values
* INFORMATION: * INFORMATION: Possible to set the values for any of the failsafes
***********************************************************************/ ***********************************************************************/
void initFlags(); void initFlags();
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Process RC channels that should operate on some flags
* INFORMATION: * 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); void flags_ProcessRcChannel(int rcChannel_ID, int value);
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Process RC channels that should operate on some flags
* INFORMATION: * 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); void flags_Set_ID(int id);
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Set flag to false(0)
* INFORMATION: * INFORMATION: Given an id set that value to false
***********************************************************************/ ***********************************************************************/
void flags_Clear_ID(int id); void flags_Clear_ID(int id);
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Toggle a flag
* INFORMATION: * INFORMATION: Given an id changes the current value of a flag to its
* opposite.
***********************************************************************/ ***********************************************************************/
void flags_Toggle_ID(int id); void flags_Toggle_ID(int id);
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Checks if a flag is set
* INFORMATION: * INFORMATION: Given an id value, check if that is set to true in the
* bitfield
***********************************************************************/ ***********************************************************************/
bool flags_IsSet_ID(int id); bool flags_IsSet_ID(int id);
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Set flag to true(1)
* INFORMATION: * INFORMATION: Given a mask set that value to true
***********************************************************************/ ***********************************************************************/
void flags_Set_MASK(int mask); void flags_Set_MASK(int mask);
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Set flag to false(0)
* INFORMATION: * INFORMATION: Given a mask set that value to false
***********************************************************************/ ***********************************************************************/
void flags_Clear_MASK(int mask); void flags_Clear_MASK(int mask);
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Toggle a flag
* INFORMATION: * INFORMATION: Given a mask changes the current value of a flag to its
* opposite.
***********************************************************************/ ***********************************************************************/
void flags_Toggle_MASK(int mask); void flags_Toggle_MASK(int mask);
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Checks if a flag is set
* INFORMATION: * INFORMATION: Given a mask value, check if that is set to true in the
* bitfield
***********************************************************************/ ***********************************************************************/
bool flags_IsSet_MASK(int mask); bool flags_IsSet_MASK(int mask);

2
UAV-ControlSystem/inc/drivers/motormix.h
View File

@ -25,7 +25,7 @@
#define MOTOR_COUNT 10 #define MOTOR_COUNT 10
// TODO: These are only temporary before merge with PID part // TODO: These are only temporary before merge with PID part
uint16_t PID_Out[3]; int16_t PID_Out[3];
// TODO: Temporary before PID // TODO: Temporary before PID
typedef enum { typedef enum {

12
UAV-ControlSystem/inc/drivers/motors.h
View File

@ -8,6 +8,8 @@
#ifndef DRIVERS_MOTORS_H_ #ifndef DRIVERS_MOTORS_H_
#define DRIVERS_MOTORS_H_ #define DRIVERS_MOTORS_H_
#define MAX_PULSE 1950
#include "stm32f4xx_revo.h" #include "stm32f4xx_revo.h"
/* Struct of motor output protocol */ /* Struct of motor output protocol */
typedef enum { typedef enum {
@ -15,6 +17,8 @@ typedef enum {
Oneshot125 Oneshot125
}motorOutput; }motorOutput;
extern bool perfromMotorCalibration;
/************************************************************************** /**************************************************************************
* BRIEF: Initializing a motor (maps a pwm signal to a certain pin) * * 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 * * 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); 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_ */ #endif /* DRIVERS_MOTORS_H_ */

2
UAV-ControlSystem/inc/drivers/sbus.h
View File

@ -25,7 +25,7 @@
#define SBUS_FRAME_SIZE 25 #define SBUS_FRAME_SIZE 25
#define SBUS_HEADER 0x0F #define SBUS_HEADER 0x0F
#define SBUS_FOOTER 0x00 #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 SBUS_MAX_CHANNEL 18 // Including two digital
#define STICK_CHANNEL_COUNT 4 #define STICK_CHANNEL_COUNT 4

12
UAV-ControlSystem/inc/stm32f4xx_revo.h
View File

@ -196,15 +196,15 @@
#define MOTOR_6_CHANNEL TIM_CHANNEL_1 #define MOTOR_6_CHANNEL TIM_CHANNEL_1
#define MOTOR_7 7 #define MOTOR_7 7
#define MOTOR_7_PIN GPIO_PIN_6 #define MOTOR_7_PIN GPIO_PIN_14
#define MOTOR_7_PORT GPIOC #define MOTOR_7_PORT GPIOB
#define MOTOR_7_TIM TIM8 #define MOTOR_7_TIM TIM12
#define MOTOR_7_CHANNEL TIM_CHANNEL_1 #define MOTOR_7_CHANNEL TIM_CHANNEL_1
#define MOTOR_8 8 #define MOTOR_8 8
#define MOTOR_8_PIN GPIO_PIN_7 #define MOTOR_8_PIN GPIO_PIN_15
#define MOTOR_8_PORT GPIOC #define MOTOR_8_PORT GPIOB
#define MOTOR_8_TIM TIM8 #define MOTOR_8_TIM TIM12
#define MOTOR_8_CHANNEL TIM_CHANNEL_2 #define MOTOR_8_CHANNEL TIM_CHANNEL_2
#define MOTOR_9 9 #define MOTOR_9 9

2
UAV-ControlSystem/inc/system_variables.h
View File

@ -14,7 +14,7 @@
#ifndef SYSTEM_VARIABLES_H_ #ifndef SYSTEM_VARIABLES_H_
#define SYSTEM_VARIABLES_H_ #define SYSTEM_VARIABLES_H_
#define EEPROM_SYS_VERSION 101 #define EEPROM_SYS_VERSION 102
#define ADC_STATE #define ADC_STATE
#include "stm32f4xx.h" #include "stm32f4xx.h"

76
UAV-ControlSystem/src/config/cli.c
View File

@ -23,6 +23,7 @@
#include <string.h> #include <string.h>
#include "utilities.h" #include "utilities.h"
#include "Scheduler/scheduler.h" #include "Scheduler/scheduler.h"
#include "drivers/motors.h"
#define cliActivateCharacter 35 #define cliActivateCharacter 35
#define commandValueError 0xFFFFFFFFFFFFFFFF #define commandValueError 0xFFFFFFFFFFFFFFFF
@ -71,6 +72,7 @@ typedef enum
ACTION_REBOOT, //Reboots the entire system ACTION_REBOOT, //Reboots the entire system
ACTION_RESET, //Resets the entire eeprom ACTION_RESET, //Resets the entire eeprom
ACTION_STATS, //gives statistics on the system ACTION_STATS, //gives statistics on the system
ACTION_MOTORCALIBRATE,
//The number of actions //The number of actions
ACTION_COUNT, //Count of the number of actions ACTION_COUNT, //Count of the number of actions
@ -91,7 +93,8 @@ const typeString commandAction_Strings[ACTION_COUNT] = {
"exit", "exit",
"reboot", "reboot",
"reset", "reset",
"stats" "stats",
"motorcalibrate"
}; };
/* String values descrbing information of a certain action */ /* 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", "| 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" "| 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 */ /* String array containing all the signature examples for each action */
@ -123,7 +127,8 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = {
" exit", " exit",
" reboot", " reboot",
" reset", " reset",
" stats" " stats",
" motorcalibrate"
}; };
/* Size values for the values a command will require */ /* Size values for the values a command will require */
@ -148,6 +153,9 @@ typedef enum
COMMAND_ID_ADC_SCALES_RIGHT, COMMAND_ID_ADC_SCALES_RIGHT,
COMMAND_ID_PID_ROLL_KP, COMMAND_ID_PID_ROLL_KP,
/* calibrate motors command */
COMMAND_ID_MOTORCALIBRATE,
/* Period values for tasks */ /* Period values for tasks */
COMMAND_ID_PERIOD_SYSTEM , COMMAND_ID_PERIOD_SYSTEM ,
COMMAND_ID_PERIOD_GYROPID, COMMAND_ID_PERIOD_GYROPID,
@ -209,13 +217,13 @@ typedef enum
COMMAND_ID_FLAG_FLIGHTMODE_GPS_MAXRANGE, COMMAND_ID_FLAG_FLIGHTMODE_GPS_MAXRANGE,
COMMAND_ID_FLAG_FLIGHTMODE_GPS_CHANNEL, COMMAND_ID_FLAG_FLIGHTMODE_GPS_CHANNEL,
COMMAND_ID_FLAG_AIRMODE_MINRANGE, COMMAND_ID_FLAG_MIXERFULLSCALE_MINRANGE,
COMMAND_ID_FLAG_AIRMODE_MAXRANGE, COMMAND_ID_FLAG_MIXERFULLSCALE_MAXRANGE,
COMMAND_ID_FLAG_AIRMODE_CHANNEL, COMMAND_ID_FLAG_MIXERFULLSCALE_CHANNEL,
COMMAND_ID_FLAG_FLIGHTMODE_2_MINRANGE, COMMAND_ID_FLAG_MIXERLOWSCALE_MINRANGE,
COMMAND_ID_FLAG_FLIGHTMODE_2_MAXRANGE, COMMAND_ID_FLAG_MIXERLOWSCALE_MAXRANGE,
COMMAND_ID_FLAG_FLIGHTMODE_2_CHANNEL, COMMAND_ID_FLAG_MIXERLOWSCALE_CHANNEL,
COMMAND_ID_FLAG_FLIGHTMODE_3_MINRANGE, COMMAND_ID_FLAG_FLIGHTMODE_3_MINRANGE,
COMMAND_ID_FLAG_FLIGHTMODE_3_MAXRANGE, 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} "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] = [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} "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} "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] = [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): case commandMask(commandSize_1, ACTION_STATS):
TransmitSystemStats(); TransmitSystemStats();
break; 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: default:
if (actionId != ACTION_NOACTION) if (actionId != ACTION_NOACTION)
TransmitCommandInstruction(actionId); TransmitCommandInstruction(actionId);

17
UAV-ControlSystem/src/config/eeprom.c
View File

@ -23,6 +23,7 @@
#include "Scheduler/scheduler.h" #include "Scheduler/scheduler.h"
#include "drivers/failsafe_toggles.h" #include "drivers/failsafe_toggles.h"
#include "drivers/motormix.h" #include "drivers/motormix.h"
#include "drivers/motors.h"
/* Reads the EEPROM version from EEPROM - Is compared to EEPROM_SYS_VERSION */ /* Reads the EEPROM version from EEPROM - Is compared to EEPROM_SYS_VERSION */
uint8_t stored_eeprom_identifier; uint8_t stored_eeprom_identifier;
@ -95,6 +96,14 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
.dataPtr = &uart1_rx_inverter, .dataPtr = &uart1_rx_inverter,
}, },
/* Motor calibrate */
[EEPROM_MOTORCALIBRATE] =
{
.size = sizeof(bool),
.dataPtr = &perfromMotorCalibration,
},
/* Task eeprom values */ /* Task eeprom values */
[EEPROM_PERIOD_SYSTEM] = [EEPROM_PERIOD_SYSTEM] =
{ {
@ -222,15 +231,15 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
.size = sizeof(flags_Configuration_t), .size = sizeof(flags_Configuration_t),
.dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_GPS]), .dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_GPS]),
}, },
[EEPROM_FLAG_AIRMODE] = [EEPROM_FLAG_MIXERFULLSCALE] =
{ {
.size = sizeof(flags_Configuration_t), .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), .size = sizeof(flags_Configuration_t),
.dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_2]), .dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_MIXERLOWSCALE]),
}, },
[EEPROM_FLAG_FLIGHTMODE_3] = [EEPROM_FLAG_FLIGHTMODE_3] =
{ {

117
UAV-ControlSystem/src/drivers/failsafe_toggles.c
View File

@ -1,11 +1,26 @@
/* /**********************************************************************
* failsafe_toggles.c * NAME: failsafe_toggles.c *
* * AUTHOR: Jonas Holmberg *
* Created on: 10 okt. 2016 * PURPOSE: Give the ability to create boolean flags and failsafe *
* Author: holmis * 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/failsafe_toggles.h"
#include "drivers/sbus.h"
/* Stuct containing values for all the flags and failsafe booleans sin the system */ /* Stuct containing values for all the flags and failsafe booleans sin the system */
boolFlags_t systemFlags = {{0}}; boolFlags_t systemFlags = {{0}};
@ -42,17 +57,17 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
.channelNumber = 0, .channelNumber = 0,
.flagId = systemFlags_flightmode_gps_id, .flagId = systemFlags_flightmode_gps_id,
}, },
[FLAG_CONFIGURATION_AIRMODE] = { [FLAG_CONFIGURATION_MIXERFULLSCALE] = {
.minRange = 0, .minRange = 0,
.maxRange = 0, .maxRange = 0,
.channelNumber = 0, .channelNumber = 0,
.flagId = systemFlags_airmode_id, .flagId = systemFlags_mixerfullscale_id,
}, },
[FLAG_CONFIGURATION_FLIGHTMODE_2] = { [FLAG_CONFIGURATION_MIXERLOWSCALE] = {
.minRange = 0, .minRange = 0,
.maxRange = 0, .maxRange = 0,
.channelNumber = 0, .channelNumber = 0,
.flagId = systemFlags_flightMode_2_id, .flagId = systemFlags_mixerlowscale_id,
}, },
[FLAG_CONFIGURATION_FLIGHTMODE_3] = { [FLAG_CONFIGURATION_FLIGHTMODE_3] = {
.minRange = 0, .minRange = 0,
@ -63,17 +78,19 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
}; };
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Could be used to set start values for some values
* INFORMATION: * INFORMATION: Possible to set the values for any of the failsafes
***********************************************************************/ ***********************************************************************/
void initFlags() void initFlags()
{ {
systemFlags.intRepresentation = 0; //Could be used to set start values for failsafes.
} }
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Process RC channels that should operate on some flags
* INFORMATION: * 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) void flags_ProcessRcChannel(int rcChannel_ID, int value)
{ {
@ -94,8 +111,42 @@ void flags_ProcessRcChannel(int rcChannel_ID, int value)
} }
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Process RC channels that should operate on some flags
* INFORMATION: * 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) void flags_Set_MASK(int mask)
{ {
@ -103,8 +154,8 @@ void flags_Set_MASK(int mask)
} }
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Set flag to false(0)
* INFORMATION: * INFORMATION: Given a mask set that value to false
***********************************************************************/ ***********************************************************************/
void flags_Clear_MASK(int mask) void flags_Clear_MASK(int mask)
{ {
@ -112,8 +163,9 @@ void flags_Clear_MASK(int mask)
} }
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Toggle a flag
* INFORMATION: * INFORMATION: Given a mask changes the current value of a flag to its
* opposite.
***********************************************************************/ ***********************************************************************/
void flags_Toggle_MASK(int mask) void flags_Toggle_MASK(int mask)
{ {
@ -121,8 +173,9 @@ void flags_Toggle_MASK(int mask)
} }
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Checks if a flag is set
* INFORMATION: * INFORMATION: Given a mask value, check if that is set to true in the
* bitfield
***********************************************************************/ ***********************************************************************/
bool flags_IsSet_MASK(int mask) bool flags_IsSet_MASK(int mask)
{ {
@ -130,8 +183,8 @@ bool flags_IsSet_MASK(int mask)
} }
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Set flag to true(1)
* INFORMATION: * INFORMATION: Given an id set that value to true
***********************************************************************/ ***********************************************************************/
void flags_Set_ID(int id) void flags_Set_ID(int id)
{ {
@ -139,8 +192,8 @@ void flags_Set_ID(int id)
} }
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Set flag to false(0)
* INFORMATION: * INFORMATION: Given an id set that value to false
***********************************************************************/ ***********************************************************************/
void flags_Clear_ID(int id) void flags_Clear_ID(int id)
{ {
@ -148,8 +201,9 @@ void flags_Clear_ID(int id)
} }
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Toggle a flag
* INFORMATION: * INFORMATION: Given an id changes the current value of a flag to its
* opposite.
***********************************************************************/ ***********************************************************************/
void flags_Toggle_ID(int id) void flags_Toggle_ID(int id)
{ {
@ -157,8 +211,9 @@ void flags_Toggle_ID(int id)
} }
/*********************************************************************** /***********************************************************************
* BRIEF: * BRIEF: Checks if a flag is set
* INFORMATION: * INFORMATION: Given an id value, check if that is set to true in the
* bitfield
***********************************************************************/ ***********************************************************************/
bool flags_IsSet_ID(int id) bool flags_IsSet_ID(int id)
{ {

103
UAV-ControlSystem/src/drivers/motormix.c
View File

@ -45,9 +45,9 @@ uint16_t motor_output[MOTOR_COUNT];
// TODO: Implement in EEPROM // TODO: Implement in EEPROM
mixerConfig_s mixerConfig = { mixerConfig_s mixerConfig = {
.minThrottle = 1050, .minThrottle = 1050,
.maxThrottle = 1950, .maxThrottle = MAX_PULSE - 100,
.minCommand = 1000, .minCommand = 1000,
.maxCommand = 2000, .maxCommand = MAX_PULSE,
.minCheck = 1010, .minCheck = 1010,
.pid_at_min_throttle = true, .pid_at_min_throttle = true,
.motorstop = false, .motorstop = false,
@ -97,14 +97,15 @@ void mix()
//uint16_t throttleIdle = mixerConfig.minThrottle + (throttleRange / 2); //uint16_t throttleIdle = mixerConfig.minThrottle + (throttleRange / 2);
int16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array 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 int16_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 int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor
uint16_t throttle = sbusChannelData.chan3; // Import throttle value from remote int16_t throttle = sbusChannelData.chan3; // Import throttle value from remote
// Might be used for some debug if necessary // Might be used for some debug if necessary
static bool motorLimitReached; 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++) 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 have enough interval for the adjustments */
// Check if we maxed out
/* Check if we maxed out */
if (RPY_MixRange > throttleRange) if (RPY_MixRange > throttleRange)
{ {
motorLimitReached = true; // Yes, we maxed out motorLimitReached = true; // Yes, we maxed out
@ -139,7 +142,12 @@ void mix()
// Apply the scaling to all outputs // Apply the scaling to all outputs
for (int i = 0; i < MOTOR_COUNT; i++) for (int i = 0; i < MOTOR_COUNT; i++)
RPY_Mix[i] = RPY_Mix[i] * mixReduction; 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 // If we have the needed range no scaling is needed
else else
{ {
@ -147,22 +155,37 @@ void mix()
/* Update min and max throttle so we can add the /* Update min and max throttle so we can add the
* calculated adjustments and still just max out */ * calculated adjustments and still just max out */
throttleMin += (RPY_MixRange / 2); throttleMin += (RPY_MixRange / 2); // Can be removed. Just made to be sure
throttleMax -= (RPY_MixRange / 2); 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 // Now we add desired throttle
for (int i = 0; i < MOTOR_COUNT; i++) for (int i = 0; i < MOTOR_COUNT; i++)
{ // Constrain in within the regulation of the mix - OBS. Constrain can be removed. Just to make sure
// Constrain in within the regulation of the mix motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax);
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);
}
} }
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 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[ROLL] * mixerUAV[i].roll + \
PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1);
// Find the maximum motor output // Find the minimum and maximum motor output
if (RPY_Mix[i] > maxMotor) maxMotor = RPY_Mix[i]; 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++) for (int i = 0; i < MOTOR_COUNT; i++)
{ {
RPY_Mix[i] -= maxThrottleDifference; // We reduce the highest overflow on all motors 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 // 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); 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++) for (int i = 0; i < MOTOR_COUNT; i++)
{ {
/* If engines are armed then give the output to the motors */ /* 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); motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
/* If not then stop motors */ /* If not then stop motors */
else else

35
UAV-ControlSystem/src/drivers/motors.c
View File

@ -12,6 +12,8 @@
#include "stm32f4xx_revo.h" #include "stm32f4xx_revo.h"
#include "drivers/pwm.h" #include "drivers/pwm.h"
#include "drivers/motors.h" #include "drivers/motors.h"
#include "drivers/failsafe_toggles.h"
#include "config/eeprom.h"
const int MotorPWMPeriode = 2000; //Micro seconds const int MotorPWMPeriode = 2000; //Micro seconds
const int MotorPWMInitPulse = 1000; const int MotorPWMInitPulse = 1000;
@ -25,6 +27,8 @@ typedef struct
uint32_t channel; //TIM_CHANNEL_1/TIM_CHANNEL_1/.. uint32_t channel; //TIM_CHANNEL_1/TIM_CHANNEL_1/..
}motorProfile; }motorProfile;
bool perfromMotorCalibration = false;
/************************************************************************** /**************************************************************************
* BRIEF: Returns the final pulse of a motor driver call * 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 * 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) 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)); 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();
}

146
UAV-ControlSystem/src/drivers/sbus.c
View File

@ -28,7 +28,7 @@
/* This instance is read by the whole system and should contain actual RX data */ /* This instance is read by the whole system and should contain actual RX data */
sbusFrame_s sbusChannelData = {0}; sbusFrame_s sbusChannelData = {0};
dma_usart_return raw_dma_data_t = {0}; dma_usart_return raw_dma_data_t;
/* Create a DMA Handler */ /* Create a DMA Handler */
usart_dma_profile dmaHandler; usart_dma_profile dmaHandler;
@ -102,69 +102,132 @@ void sbus_read()
{ {
// Holds what we've read so far // Holds what we've read so far
static uint8_t sbus_arr[SBUS_FRAME_SIZE]; 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 int sbus_arr_iterator = SBUS_FRAME_SIZE;
static bool stop_bit_read = false; static bool stop_bit_read = false;
// If continue only if we get new data from DMA // If continue only if we get new data from DMA
if (raw_dma_data_t.new_data) if (raw_dma_data_t.new_data)
{ {
for (int i = 0; i < USART1_SBUS_DMA_SIZE; i++) 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 // 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; //message_it = (raw_dma_data_t.buff[i] == ((uint8_t)SBUS_HEADER)) ? 1 : 0;
stop_bit_read = false; 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 // 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 (msg == (uint8_t)SBUS_HEADER && new_header == false)
// 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; 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); if ((message_it) == SBUS_FRAME_SIZE)
sbusChannelData.chan2 = SBUS_UNIT_CONV(sbusChannelData.chan2); {
sbusChannelData.chan3 = SBUS_UNIT_CONV(sbusChannelData.chan3); missedMsg++;
sbusChannelData.chan4 = SBUS_UNIT_CONV(sbusChannelData.chan4); if (msg == (uint8_t)SBUS_FOOTER)
sbusChannelData.chan5 = SBUS_UNIT_CONV(sbusChannelData.chan5); {
sbusChannelData.chan6 = SBUS_UNIT_CONV(sbusChannelData.chan6); message_it = 0;
sbusChannelData.chan7 = SBUS_UNIT_CONV(sbusChannelData.chan7); missedMsg--;
sbusChannelData.chan8 = SBUS_UNIT_CONV(sbusChannelData.chan8); //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 // Linear fitting
sbusChannelData.chan9 = SBUS_UNIT_CONV(sbusChannelData.chan9); sbusChannelData.chan1 = SBUS_UNIT_CONV(sbusChannelData.chan1);
sbusChannelData.chan10 = SBUS_UNIT_CONV(sbusChannelData.chan10); sbusChannelData.chan2 = SBUS_UNIT_CONV(sbusChannelData.chan2);
sbusChannelData.chan11 = SBUS_UNIT_CONV(sbusChannelData.chan11); sbusChannelData.chan3 = SBUS_UNIT_CONV(sbusChannelData.chan3);
sbusChannelData.chan12 = SBUS_UNIT_CONV(sbusChannelData.chan12); sbusChannelData.chan4 = SBUS_UNIT_CONV(sbusChannelData.chan4);
sbusChannelData.chan13 = SBUS_UNIT_CONV(sbusChannelData.chan13); sbusChannelData.chan5 = SBUS_UNIT_CONV(sbusChannelData.chan5);
sbusChannelData.chan14 = SBUS_UNIT_CONV(sbusChannelData.chan14); sbusChannelData.chan6 = SBUS_UNIT_CONV(sbusChannelData.chan6);
sbusChannelData.chan15 = SBUS_UNIT_CONV(sbusChannelData.chan15); sbusChannelData.chan7 = SBUS_UNIT_CONV(sbusChannelData.chan7);
sbusChannelData.chan16 = SBUS_UNIT_CONV(sbusChannelData.chan16); 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) // // Copy next byte into the sbus_arr
sbus_arr[sbus_arr_iterator] = raw_dma_data_t.buff[i]; // if (sbus_arr_iterator < SBUS_FRAME_SIZE)
sbus_arr_iterator++; // 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; toReturn = frame.flag_DChannel_18;
break; break;
#endif #endif
default:
toReturn = 0;
break;
} }
return toReturn; return toReturn;
} }

6
UAV-ControlSystem/src/main.c
View File

@ -29,6 +29,7 @@
#include "drivers/motors.h" #include "drivers/motors.h"
#include "Flight/pid.h" #include "Flight/pid.h"
/************************************************************************** /**************************************************************************
* BRIEF: Should contain all the initializations of the system, needs to * BRIEF: Should contain all the initializations of the system, needs to
* run before the scheduler. * run before the scheduler.
@ -49,12 +50,13 @@ void init_system()
readEEPROM(); readEEPROM();
//initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble //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 //init sbus, using USART1
sbus_init(); sbus_init();
//init motors to run with oneshot 125 //init motors to run with oneshot 125, small delay
HAL_Delay(1000);
pwmEnableAllMotors(Oneshot125); pwmEnableAllMotors(Oneshot125);
pidInit(); pidInit();

11
UAV-ControlSystem/src/tasks_main.c
View File

@ -85,10 +85,13 @@ void systemTaskRx(void)
sbusFrame_s frame = sbusChannelData; sbusFrame_s frame = sbusChannelData;
/* Process channel data for switches and toggles on the controller, starts after "stick" channels */ /* 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 // 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)); // 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 //temporary send data from the RC directly form the RC
RawRcCommand.Roll = frame.chan1; RawRcCommand.Roll = frame.chan1;