From 9b0d35a5794ba4e336027d2962fa61a7f1e21724 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Thu, 13 Oct 2016 10:00:48 +0200 Subject: [PATCH] Small changes to the motermix to perform testing --- UAV-ControlSystem/src/drivers/motormix.c | 16 +++++++-------- UAV-ControlSystem/src/drivers/motors.c | 8 ++++---- UAV-ControlSystem/src/main.c | 12 ++++++++---- UAV-ControlSystem/src/tasks_main.c | 25 ++++++++++++++---------- 4 files changed, 35 insertions(+), 26 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 574ecff..433dcca 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -44,10 +44,10 @@ uint16_t motor_output[MOTOR_COUNT]; /* Default values for the mixerConfig */ // TODO: Implement in EEPROM mixerConfig_s mixerConfig = { - .minThrottle = 1000, - .maxThrottle = 2000, - .minCommand = 1050, - .maxCommand = 1950, + .minThrottle = 1050, + .maxThrottle = 1950, + .minCommand = 1000, + .maxCommand = 2000, .minCheck = 1010, .pid_at_min_throttle = true, .motorstop = false, @@ -122,7 +122,7 @@ void mix() } - uint16_t throttleRange = mixerConfig.maxThrottle - mixerConfig.minThrottle; // The throttle range we have with current defines + 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 @@ -156,7 +156,7 @@ void mix() 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, 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); } } @@ -185,7 +185,7 @@ void mix() { RPY_Mix[i] -= maxThrottleDifference; // We reduce the highest overflow on all motors - RPY_Mix[i] = constrain(RPY_Mix[i], mixerConfig.minThrottle, mixerConfig.maxThrottle); + RPY_Mix[i] = constrain(RPY_Mix[i], mixerConfig.minThrottle, mixerConfig.maxCommand); if (throttle < mixerConfig.minCheck) { if (mixerConfig.motorstop) @@ -213,7 +213,7 @@ void mix() motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand); /* If not then stop motors */ else - motor_output[i] = mixerConfig.minThrottle; + motor_output[i] = mixerConfig.minCommand; /* Update actuators */ pwmAdjustSpeedOfMotor( i + 1 /* Motors start from Motor 1 */,motor_output[i]); diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c index 8ec92bb..75a85b3 100644 --- a/UAV-ControlSystem/src/drivers/motors.c +++ b/UAV-ControlSystem/src/drivers/motors.c @@ -32,7 +32,7 @@ typedef struct **************************************************************************/ uint16_t checkPulse(uint16_t pulse) { - return ((pulse/MotorPWMPeriode)*100 < 94)? pulse: MotorPWMPeriode*0.94; + return ((pulse/MotorPWMPeriode)*100 < 98)? pulse: MotorPWMPeriode*0.98; } /************************************************************************** @@ -126,13 +126,13 @@ uint32_t setMotorOutput(motorOutput motorOutput) switch(motorOutput) { case PWM: - prescaler = 88; + prescaler = 144;//88 break; case Oneshot125: - prescaler = 11; + prescaler = 17; break; default: - prescaler = 11; + prescaler = 17; break; } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index af2da2d..0e884c5 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -26,6 +26,7 @@ #include "drivers/failsafe_toggles.h" #include "drivers/sbus.h" #include "drivers/motormix.h" +#include "drivers/motors.h" /************************************************************************** * BRIEF: Should contain all the initializations of the system, needs to @@ -47,11 +48,14 @@ void init_system() readEEPROM(); - //initialize the CLI - cliInit(USART1); + //initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble + cliInit(USART3); - //init sbus - //sbus_init(); + //init sbus, using USART1 + sbus_init(); + + //init motors to run with oneshot 125 + pwmEnableAllMotors(Oneshot125); #ifdef USE_LEDS //Initialize the on board leds diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 7ea0c8f..2e6b1cc 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -39,21 +39,21 @@ #include "drivers/failsafe_toggles.h" #include "drivers/I2C.h" #include "drivers/accel_gyro.h" +#include "drivers/motormix.h" void systemTaskGyroPid(void) { //Read gyro and update PID and finally update the motors. The most important task in the system - uint8_t c = 102; - if (flags_IsSet_ID(systemFlags_Failsafe_noRcReceived_id)) - usart_transmit(&cliUsart, &c, 1, 1000000000); + //call the motor mix + mix(); } void systemTaskAccelerometer(void) { //update the accelerometer data - uint8_t c = 97; - usart_transmit(&cliUsart, &c, 1, 1000000000); +// uint8_t c = 97; +// usart_transmit(&cliUsart, &c, 1, 1000000000); } void systemTaskAttitude(void) @@ -72,11 +72,16 @@ void systemTaskRx(void) sbusFrame_s frame = sbusChannelData; /* Process channel data for switches and toggles on the controller, starts after "stick" channels */ - for (int i = 1; i < STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT; i++) //ToDo: add define for the number of channels to process, /if not work change i to start at numb of stick channels + 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)); } + //temporary send data from the RC directly form the RC + PID_Out[0] = frame.chan1 - 1500; + PID_Out[1] = frame.chan2 - 1500; + PID_Out[2] = frame.chan4 - 1500; + } bool systemTaskRxCheck(uint32_t currentDeltaTime) @@ -128,8 +133,8 @@ bool systemTaskRxCliCheck(uint32_t currentDeltaTime) void systemTaskSerial(void) { - uint8_t c = 118; - usart_transmit(&cliUsart, &c, 1, 1000000000); +// uint8_t c = 118; +// usart_transmit(&cliUsart, &c, 1, 1000000000); if (flags_IsSet_ID(systemFlags_armed_id)) ledOnInverted(Led0_PIN, Led0_GPIO_PORT); else @@ -140,8 +145,8 @@ void systemTaskSerial(void) void systemTaskBattery(void) { //Keep track of the battery level of the system - uint8_t c = 98; - usart_transmit(&cliUsart, &c, 1, 1000000000); +// uint8_t c = 98; +// usart_transmit(&cliUsart, &c, 1, 1000000000); if (flags_IsSet_MASK((systemFlags_flightmode_acceleromter_mask | systemFlags_armed_mask))) ledOnInverted(Led1, Led1_GPIO_PORT); else