From d72e7e099b829a38975dbcde72a69c6d04f96bc8 Mon Sep 17 00:00:00 2001 From: philsson Date: Mon, 17 Oct 2016 14:33:00 +0200 Subject: [PATCH] New mixer mode implemented with low scaling. Renaming of Airmode into Full scale Implemented new modes in cli Fixed bugg in motors.c cutting out max value --- UAV-ControlSystem/inc/config/eeprom.h | 4 +- .../inc/drivers/failsafe_toggles.h | 16 ++--- UAV-ControlSystem/inc/drivers/motors.h | 2 + UAV-ControlSystem/src/config/cli.c | 38 +++++----- UAV-ControlSystem/src/config/eeprom.c | 8 +-- .../src/drivers/failsafe_toggles.c | 8 +-- UAV-ControlSystem/src/drivers/motormix.c | 69 +++++++++++-------- UAV-ControlSystem/src/drivers/motors.c | 2 +- 8 files changed, 81 insertions(+), 66 deletions(-) diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 3b57398..06536a4 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -187,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..382a578 100644 --- a/UAV-ControlSystem/inc/drivers/failsafe_toggles.h +++ b/UAV-ControlSystem/inc/drivers/failsafe_toggles.h @@ -27,8 +27,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 +43,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 +67,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,8 +90,8 @@ 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 diff --git a/UAV-ControlSystem/inc/drivers/motors.h b/UAV-ControlSystem/inc/drivers/motors.h index d7f32a1..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 { diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index a1ab3fc..dcf7d2f 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -217,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, @@ -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} }, - [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] = @@ -498,8 +498,6 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { }, - - }; /*********************************************************************** diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index 81a4425..45e73df 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -231,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..ec7b139 100644 --- a/UAV-ControlSystem/src/drivers/failsafe_toggles.c +++ b/UAV-ControlSystem/src/drivers/failsafe_toggles.c @@ -42,17 +42,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, diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index b370f55..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, @@ -104,7 +104,8 @@ void mix() // 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++) { @@ -129,9 +130,9 @@ void mix() int16_t throttleRange = throttleMax - throttleMin; // The throttle range we have with current defines 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 maxed out + + /* Check if we maxed out */ if (RPY_MixRange > throttleRange) { motorLimitReached = true; // Yes, we maxed out @@ -140,21 +141,13 @@ void mix() float mixReduction = (float) throttleRange / RPY_MixRange; // Apply the scaling to all outputs for (int i = 0; i < MOTOR_COUNT; i++) - { 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) 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 else { @@ -184,15 +177,15 @@ void mix() // Now we add desired throttle 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 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 { @@ -202,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 @@ -230,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); } } diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c index 17902b6..b2a83d9 100644 --- a/UAV-ControlSystem/src/drivers/motors.c +++ b/UAV-ControlSystem/src/drivers/motors.c @@ -36,7 +36,7 @@ bool perfromMotorCalibration = false; **************************************************************************/ uint16_t checkPulse(uint16_t pulse) { - return ((pulse/MotorPWMPeriode)*100 < 98)? pulse: MotorPWMPeriode*0.98; + return (pulse > MAX_PULSE) ? MAX_PULSE : pulse; } /**************************************************************************