/******** 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 ); }