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

Fixing RC problems on header in message

Browse Source
This commit is contained in:
philsson 2016-10-13 14:04:29 +02:00
parent 9b0d35a579
commit 0df214b68a
4 changed files with 78 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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 {

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 * 5 + 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

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

@ -97,9 +97,9 @@ 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;
@ -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 int16_t RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min; // Range of the desired mixer outputs
uint16_t throttleMin = mixerConfig.minThrottle; // Import system variable int16_t throttleMin = mixerConfig.minThrottle; // Import system variable
uint16_t throttleMax = mixerConfig.maxThrottle; // Import 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 */ /* Check if we have enough interval for the adjustments */
@ -156,8 +157,8 @@ void mix()
for (int i = 0; i < MOTOR_COUNT; i++) for (int i = 0; i < MOTOR_COUNT; i++)
{ {
// Constrain in within the regulation of the mix // 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] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax);
motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand); //motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
} }
} }
else // Airmode not active else // Airmode not active

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

@ -102,6 +102,7 @@ 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 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;
@ -109,61 +110,84 @@ void sbus_read()
// 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++)
{ {
// 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; // 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 the expected byte is stop byte, then we overwrite to the return value. if (message_it -1 < SBUS_FRAME_SIZE)
if (sbus_arr_iterator == SBUS_FRAME_SIZE - 1)
{ {
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 {
message_it = 0;
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); //stop_bit_read = true;
sbusChannelData.chan3 = rx_truncate(sbusChannelData.chan3); // If the expected byte is stop byte, then we overwrite to the return value.
sbusChannelData.chan4 = rx_truncate(sbusChannelData.chan4); //if (sbus_arr_iterator == SBUS_FRAME_SIZE - 1)
sbusChannelData.chan5 = rx_truncate(sbusChannelData.chan5); //{
sbusChannelData.chan6 = rx_truncate(sbusChannelData.chan6); sbusChannelData = *(sbusFrame_s*)sbus_arr;
sbusChannelData.chan7 = rx_truncate(sbusChannelData.chan7);
sbusChannelData.chan8 = rx_truncate(sbusChannelData.chan8); // 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) // // 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++;
} }
} }
} }