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

New mixer mode implemented with low scaling. Renaming of Airmode into Full scale

Browse Source 
Implemented new modes in cli
Fixed bugg in motors.c cutting out max value
This commit is contained in:
philsson 2016-10-17 14:33:00 +02:00
parent 82c8040520
commit d72e7e099b
8 changed files with 81 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -187,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 */

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

@ -27,8 +27,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 +43,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 +67,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,8 +90,8 @@ 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

2
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 {

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

@ -217,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,
@ -458,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] =
@ -498,8 +498,6 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
}, },
}; };
/*********************************************************************** /***********************************************************************

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

@ -231,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] =
{ {

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

@ -42,17 +42,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,

69
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,
@ -104,7 +104,8 @@ void mix()
// 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++)
{ {
@ -129,9 +130,9 @@ void mix()
int16_t throttleRange = throttleMax - throttleMin; // The throttle range we have with current defines int16_t throttleRange = throttleMax - throttleMin; // The throttle range we have with current defines
int16_t throttleMid = (throttleMax + throttleMin) / 2; int16_t throttleMid = (throttleMax + throttleMin) / 2;
//TODO: Seems to be causing an error when maxing any of the stick with low throttle
/* 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
@ -140,21 +141,13 @@ void mix()
float mixReduction = (float) throttleRange / RPY_MixRange; float mixReduction = (float) throttleRange / RPY_MixRange;
// 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;
//TEMPFIX recalculate
// 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];
}
//Temp fix may not be right - Set throttle to exakt half (As this craft is symmetric) //Temp fix may not be right - Set throttle to exakt half (As this craft is symmetric)
throttle = mixerConfig.minThrottle + mixReduction * RPY_Mix_Max; throttle = mixerConfig.minThrottle + mixReduction * RPY_Mix_Max;
//throttle += mixerConfig.minThrottle + throttleRange / 2;
// RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min;
// throttleMin += (RPY_MixRange / 2);
// throttleMax -= (RPY_MixRange / 2);
} }
// If we have the needed range no scaling is needed // If we have the needed range no scaling is needed
else else
{ {
@ -184,15 +177,15 @@ void mix()
// 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 - OBS. Constrain can be removed. Just to make sure
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, throttleMax);
//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
{ {
@ -202,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
@ -230,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);
} }
} }

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

@ -36,7 +36,7 @@ bool perfromMotorCalibration = false;
**************************************************************************/ **************************************************************************/
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;
} }
/************************************************************************** /**************************************************************************