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

2
UAV-ControlSystem/inc/drivers/sbus.h
View File

@ -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

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

@ -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

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

@ -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++;
}
}
}