/******** Debug_NetworkTransceiver_MaxRetries ******************************
// FAILED - turn off receiver to max out retries
// Input: none
// Output: none
// ------------------------------------------------------------------------*/
void Debug_NetworkTransceiver_MaxRetries ( void )
{
unsigned char status, completed_loop;
UARTprintf ( "Start Max Retries test in 3 seconds. Turn off receiver now...3>" );
Delay_S(1);
UARTprintf ( "2>" );
Delay_S(1);
UARTprintf ( "1>" );
Delay_S(1);
UARTprintf ( "0\n" );
Delay_S(1);
completed_loop = TransceiverPacket_Producer(1, &status);
if ( status == ERROR_MAX_RETRIES && 0 == completed_loop )
UARTprintf ("MaxRetries test PASSED: Status = 0x%x\n", status);
else
UARTprintf ("MaxRetries test FAILED: Status = 0x%x\n", status);
UARTprintf ("-----------------------------------------\n");
switch ( testID )
{
case '7':
break;
default:
xTaskCreate( Debug_NetworkTransceiver, ( signed portCHAR * ) "Debug_NetworkTransceiver",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
}
vTaskDelete ( NULL );
}
void Transceiver_Locker ( void )
{
while ( xSemaphoreTake(Transceiver_Mutex, portMAX_DELAY) != pdTRUE );
UARTprintf ( "Locker thread grabbed lock, release in 3 seconds...3>" );
Delay_S(1);
UARTprintf ( "2>" );
Delay_S(1);
UARTprintf ( "1>" );
Delay_S(1);
UARTprintf ( "0\n" );
Delay_S(1);
while ( xSemaphoreGive(Transceiver_Mutex) != pdTRUE );
UARTprintf ( "Locker thread released lock\n" );
vTaskDelete ( NULL );
}
/******** Debug_NetworkTransceiver_FullQueue *******************************
// FAILED - test failure on receiving more packets than queue size
// Input: none
// Output: none
// ------------------------------------------------------------------------*/
void Debug_NetworkTransceiver_FullQueue ( void )
{
unsigned char status;
unsigned char tx[TRANSCEIVER_MAX_PAYLOAD];
Transceiver_SetCallBack ( FULL_QUEUE_CALL_BACK, Dummy_FullQueueCallBack );
GPIOPinIntDisable ( nrf24l01_IRQ_IOREGISTER, nrf24l01_IRQ_PIN );
while ( pdPASS == xQueueSend(Transceiver_RX_Queue, tx, (portTickType)0) );
GPIOPinIntEnable ( nrf24l01_IRQ_IOREGISTER, nrf24l01_IRQ_PIN );
UARTprintf ( "Queue is full. Turn on transmitter and send a packet now...5>" );
Delay_S(1);
UARTprintf ( "4>" );
Delay_S(1);
UARTprintf ( "3>" );
Delay_S(1);
UARTprintf ( "2>" );
Delay_S(1);
UARTprintf ( "1>" );
Delay_S(1);
UARTprintf ( "0\n" );
Delay_S(1);
switch ( testID )
{
case '0':
xTaskCreate( Debug_NetworkTransceiver_EmptyQueue, ( signed portCHAR * ) "Debug_NetworkTransceiver_EmptyQueue",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
case '7':
break;
default:
Delay_S ( 1 );
xTaskCreate( Debug_NetworkTransceiver, ( signed portCHAR * ) "Debug_NetworkTransceiver",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
}
vTaskDelete ( NULL );
}
/******** Debug_NetworkTransceiver_EmptyQueue ******************************
// FAILED - receive packet from empty queue
// Input: none
// Output: none
// ------------------------------------------------------------------------*/
void Debug_NetworkTransceiver_EmptyQueue ( void )
{
unsigned char status;
unsigned char rx[TRANSCEIVER_MAX_PAYLOAD];
TransceiverPacket pkt;
Transceiver_SetCallBack ( EMPTY_QUEUE_CALL_BACK, Dummy_EmptyQueueCallBack );
GPIOPinIntDisable ( nrf24l01_IRQ_IOREGISTER, nrf24l01_IRQ_PIN );
while ( pdPASS == xQueueReceive(Transceiver_RX_Queue, rx, (portTickType)0) );
status = Transceiver_ReceiveMessage ( &pkt );
GPIOPinIntEnable ( nrf24l01_IRQ_IOREGISTER, nrf24l01_IRQ_PIN );
if ( status == ERROR_QUEUE_EMPTY )
UARTprintf ("EmptyQueue test PASSED: Status = 0x%x\n", status);
else
UARTprintf ("EmptyQueue test FAILED: Status = 0x%x\n", status);
UARTprintf ("-----------------------------------------\n");
switch ( testID )
{
case '0':
xTaskCreate( Debug_NetworkTransceiver_MaxRetries, ( signed portCHAR * ) "Debug_NetworkTransceiver_MaxRetries",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
case '7':
break;
default:
Delay_S ( 1 );
xTaskCreate( Debug_NetworkTransceiver, ( signed portCHAR * ) "Debug_NetworkTransceiver",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
}
vTaskDelete ( NULL );
}
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
float norm;
float vx, vy, vz;
float ex, ey, ez;
float Pitch,Rool,Yaw;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity
vx = 2*(q11*q33 - q00*q22);
vy = 2*(q00*q11 + q22*q33);
vz = q00*q00 - q11*q11 - q22*q22 + q33*q33;
// error is sum of cross product between reference direction of field and direction measured by sensor
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// integrate quaternion rate and normalise
q00 = q00 + (-q11*gx - q22*gy - q33*gz)*halfT;
q11 = q11 + (q00*gx + q22*gz - q33*gy)*halfT;
q22 = q22 + (q00*gy - q11*gz + q33*gx)*halfT;
q33 = q33 + (q00*gz + q11*gy - q22*gx)*halfT;
// normalise quaternion
norm = sqrt(q00*q00 + q11*q11 + q22*q22 + q33*q33);
q00 = q00/ norm;
q11 = q11 / norm;
q22 = q22 / norm;
q33 = q33 / norm;
Pitch = (float)asin(-2 * q11 * q33 + 2 * q00* q22)* 57.3; // pitch
Rool = (float)atan2(2 * q22 * q33 + 2 * q00 * q11, -2 * q11 * q11 - 2 * q22* q22 + 1)* 57.3; // rollv
//Yaw = (float)(atan2(2 * q11 * q22 + 2 * q00 * q33, (-2) * q22 * q22 + (-2) * q33 * q33 + 1) * 57.3);
/* if(Pitch-0x03>0)
{
USART_SendData(USART1,0x00);
}
else if(Pitch-0x03<0)
{
USART_SendData(USART1,0x01);
}
Delay_S(1);
if(Rool-0xB0>0)
{
USART_SendData(USART1,0x00);
}
else if(Rool-0xB0<0)
{
USART_SendData(USART1,0x01);
}
Delay_S(1);
if(Yaw-0xB1>0)
{
USART_SendData(USART1,0x00);
}
else if(Yaw-0xB1<0)
{
USART_SendData(USART1,0x01);
} */
USART_SendData(USART1,Pitch);
Delay_S(1);
USART_SendData(USART1,Rool);
Delay_S(1);
USART_SendData(USART1,Yaw);
}
/******** Debug_NetworkTransceiver *****************************************
// setup necessary debug instrument, run the tests
// Input: none
// Output: none
// ------------------------------------------------------------------------*/
void Debug_NetworkTransceiver ( void )
{
unsigned char status, request_loop, completed_loop;
UARTprintf ( "\n\n--- Network Transceiver Test Suite ---\n"
"0) ALL\n"
"1) EmptyQueue\n"
"2) FullQueue\n"
"3) Invalid Packet\n"
"4) Lock\n"
"5) Positive\n"
"6) Max Retries\n"
"7) Quit\n"
"> ");
UARTgets ( &testID, 2 );
switch ( testID )
{
case '0':
UARTprintf ( "Test suite begin in 5 seconds. Turn on another device now...5>" );
Delay_S(1);
UARTprintf ( "4>" );
Delay_S(1);
UARTprintf ( "3>" );
Delay_S(1);
UARTprintf ( "2>" );
Delay_S(1);
UARTprintf ( "1>" );
Delay_S(1);
UARTprintf ( "0\n" );
Delay_S(1);
xTaskCreate( Debug_NetworkTransceiver_InvalidPakcet, ( signed portCHAR * ) "Debug_NetworkTransceiver_InvalidPakcet",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
case '1':
xTaskCreate( Debug_NetworkTransceiver_EmptyQueue, ( signed portCHAR * ) "Debug_NetworkTransceiver_EmptyQueue",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
case '2':
xTaskCreate( Debug_NetworkTransceiver_FullQueue, ( signed portCHAR * ) "Debug_NetworkTransceiver_FullQueue",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
case '3':
xTaskCreate( Debug_NetworkTransceiver_InvalidPakcet, ( signed portCHAR * ) "Debug_NetworkTransceiver_InvalidPakcet",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
case '4':
xTaskCreate( Transceiver_Locker, ( signed portCHAR * ) "Transceiver_Locker",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
xTaskCreate( Debug_NetworkTransceiver_Lock, ( signed portCHAR * ) "Debug_NetworkTransceiver_Lock",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
case '5':
xTaskCreate( Debug_NetworkTransceiver_Positive, ( signed portCHAR * ) "Debug_NetworkTransceiver_Positive",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
case '6':
xTaskCreate( Debug_NetworkTransceiver_MaxRetries, ( signed portCHAR * ) "Debug_NetworkTransceiver_MaxRetries",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
case '7':
UARTprintf ( "Goodbye!\n\n" );
break;
default:
UARTprintf ( "Invalid choice\n\n" );
xTaskCreate( Debug_NetworkTransceiver, ( signed portCHAR * ) "Debug_NetworkTransceiver",
DEFAULT_STACK_SIZE, NULL, DEFAULT_PRIORITY, NULL );
break;
}
vTaskDelete ( NULL );
}
