diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h index 498bb8d..066a92a 100644 --- a/UAV-ControlSystem/inc/drivers/motormix.h +++ b/UAV-ControlSystem/inc/drivers/motormix.h @@ -25,7 +25,7 @@ #define MOTOR_COUNT 10 // TODO: These are only temporary before merge with PID part -uint16_t PID_Out[3]; +int16_t PID_Out[3]; // TODO: Temporary before PID typedef enum { diff --git a/UAV-ControlSystem/inc/drivers/sbus.h b/UAV-ControlSystem/inc/drivers/sbus.h index 8076b61..061600b 100644 --- a/UAV-ControlSystem/inc/drivers/sbus.h +++ b/UAV-ControlSystem/inc/drivers/sbus.h @@ -25,7 +25,7 @@ #define SBUS_FRAME_SIZE 25 #define SBUS_HEADER 0x0F #define SBUS_FOOTER 0x00 -#define USART1_SBUS_DMA_SIZE SBUS_FRAME_SIZE + 1 // sbus package is 176 bits (22 bytes) +#define USART1_SBUS_DMA_SIZE SBUS_FRAME_SIZE * 5 + 1 // sbus package is 176 bits (22 bytes) #define SBUS_MAX_CHANNEL 18 // Including two digital #define STICK_CHANNEL_COUNT 4 diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 433dcca..c6e7e64 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -97,9 +97,9 @@ void mix() //uint16_t throttleIdle = mixerConfig.minThrottle + (throttleRange / 2); int16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array - uint16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor - uint16_t RPY_Mix_Max = 0; // Maximum desired command for any motor - uint16_t throttle = sbusChannelData.chan3; // Import throttle value from remote + int16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor + int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor + int16_t throttle = sbusChannelData.chan3; // Import throttle value from remote // Might be used for some debug if necessary static bool motorLimitReached; @@ -122,10 +122,11 @@ 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 /* Check if we have enough interval for the adjustments */ @@ -156,8 +157,8 @@ 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, mixerConfig.maxCommand); - motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand); + 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 diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c index e518afd..440f372 100644 --- a/UAV-ControlSystem/src/drivers/sbus.c +++ b/UAV-ControlSystem/src/drivers/sbus.c @@ -102,6 +102,7 @@ void sbus_read() { // Holds what we've read so far static uint8_t sbus_arr[SBUS_FRAME_SIZE]; + static uint8_t message_it = 0; static int sbus_arr_iterator = SBUS_FRAME_SIZE; static bool stop_bit_read = false; @@ -109,61 +110,84 @@ void sbus_read() // If continue only if we get new data from DMA if (raw_dma_data_t.new_data) { + for (int i = 0; i < USART1_SBUS_DMA_SIZE; i++) { // Look for the beginning of a sbus frame - if (raw_dma_data_t.buff[i] == (uint8_t)SBUS_HEADER && stop_bit_read) + if ( message_it == 0 ) //&& stop_bit_read) { - sbus_arr_iterator = 0; - stop_bit_read = false; + message_it = (raw_dma_data_t.buff[i] == ((uint8_t)SBUS_HEADER)) ? 1 : 0; +// sbus_arr_iterator = 0; +// stop_bit_read = false; } // Look for the end of sbus frame - else if(raw_dma_data_t.buff[i] == (uint8_t)SBUS_FOOTER) + //else if(raw_dma_data_t.buff[i] == (uint8_t)SBUS_FOOTER) + else { - stop_bit_read = true; - // If the expected byte is stop byte, then we overwrite to the return value. - if (sbus_arr_iterator == SBUS_FRAME_SIZE - 1) + + if (message_it -1 < SBUS_FRAME_SIZE) { - sbusChannelData = *(sbusFrame_s*)sbus_arr; + sbus_arr[message_it - 1] = raw_dma_data_t.buff[i]; + message_it++; + } + if (message_it - 1 == SBUS_FRAME_SIZE) + { + if (raw_dma_data_t.buff[i] == SBUS_FOOTER) - // Linear fitting - - sbusChannelData.chan1 = SBUS_UNIT_CONV(sbusChannelData.chan1); - sbusChannelData.chan2 = SBUS_UNIT_CONV(sbusChannelData.chan2); - sbusChannelData.chan3 = SBUS_UNIT_CONV(sbusChannelData.chan3); - sbusChannelData.chan4 = SBUS_UNIT_CONV(sbusChannelData.chan4); - sbusChannelData.chan5 = SBUS_UNIT_CONV(sbusChannelData.chan5); - sbusChannelData.chan6 = SBUS_UNIT_CONV(sbusChannelData.chan6); - sbusChannelData.chan7 = SBUS_UNIT_CONV(sbusChannelData.chan7); - sbusChannelData.chan8 = SBUS_UNIT_CONV(sbusChannelData.chan8); - - // TODO: 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 + { + message_it = 0; - 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); + + //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; + + // Linear fitting + + sbusChannelData.chan1 = SBUS_UNIT_CONV(sbusChannelData.chan1); + sbusChannelData.chan2 = SBUS_UNIT_CONV(sbusChannelData.chan2); + sbusChannelData.chan3 = SBUS_UNIT_CONV(sbusChannelData.chan3); + sbusChannelData.chan4 = SBUS_UNIT_CONV(sbusChannelData.chan4); + sbusChannelData.chan5 = SBUS_UNIT_CONV(sbusChannelData.chan5); + sbusChannelData.chan6 = SBUS_UNIT_CONV(sbusChannelData.chan6); + sbusChannelData.chan7 = SBUS_UNIT_CONV(sbusChannelData.chan7); + sbusChannelData.chan8 = SBUS_UNIT_CONV(sbusChannelData.chan8); + + // TODO: 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 + message_it = 0; + } } - // Copy next byte into the sbus_arr - if (sbus_arr_iterator < SBUS_FRAME_SIZE) - sbus_arr[sbus_arr_iterator] = raw_dma_data_t.buff[i]; - sbus_arr_iterator++; + +// // Copy next byte into the sbus_arr +// if (sbus_arr_iterator < SBUS_FRAME_SIZE) +// sbus_arr[sbus_arr_iterator] = raw_dma_data_t.buff[i]; +// sbus_arr_iterator++; } } }