rename*d*_include_HEADERS typo again in lpcanvca

[LPC2xxx_and_RobotSpejbl.git] / app / rama_dam / rama_dam.c

/********************************************************/
/*      Data Acqusition software for the DAM module     */
/*              RAMA UAV control system                 */
/*                                                      */
/*              ver. 5.3 release                        */
/*                                                      */
/* (c) 2006, 2007 Ondrej Spinka, DCE FEE CTU Prague     */
/*                                                      */
/* no animals were harmed during development/testing    */
/* of this software product, except the author himself  */
/********************************************************/

#include <cpu_def.h>
#include <types.h>
#include <lpc21xx.h>
#include <string.h>
#include <periph/can.h>
#include <periph/uart_zen.h>
#include "pwm.h"

/*! @defgroup defines Constants Definitions
* @{
*/

/*!\def TRUE
* Formal definition of logical "TRUE" value.
*/
#define TRUE 1

/*!\def WATCHDOG_INIT
* Watchdog counter initialization/reset value.
*/
/* 0x0003D090 corresponds to 100ms reset timeout */
#define WATCHDOG_INIT 0x0003D090

/*! @defgroup MEMMAP_EXC Exception Mapping Definitions
* @ingroup defines
* These constants serve to set the exception mapping either to FLASH (0x01) or RAM (0x02).
* @{
*/
#define MEMMAP_EXC_FLASH 0x01
#define MEMMAP_EXC_RAM 0x02
/*! @} */

/*!\def STATUS_LED
* Defines that status LEDs exist on the target board.
*/
#define STATUS_LED

/*! @defgroup led_defines Status LEDs Definitions
* @ingroup defines
* These constants define the IO ports and specific bits to manipulate respective status LEDs.
* @{
*/
#ifdef STATUS_LED

#define LED_IMU_PINSEL PINSEL2
#define LED_IMU_PINSEL_VAL 0xFFFFFFF3
#define LED_IMU_IODIR IODIR1
#define LED_IMU_IOSET IOSET1
#define LED_IMU_IOCLR IOCLR1
#define LED_IMU_IOBIT 0x00080000

#define LED_GPS_PINSEL PINSEL1
#define LED_GPS_PINSEL_VAL 0xFFFFCFFF
#define LED_GPS_IODIR IODIR0
#define LED_GPS_IOSET IOSET0
#define LED_GPS_IOCLR IOCLR0
#define LED_GPS_IOBIT 0x00400000

#define LED_MSH_PINSEL PINSEL1
#define LED_MSH_PINSEL_VAL 0xFFF3FFFF;
#define LED_MSH_IODIR IODIR0
#define LED_MSH_IOSET IOSET0
#define LED_MSH_IOCLR IOCLR0
#define LED_MSH_IOBIT 0x00800000

#define LED_ERR_PINSEL PINSEL1
#define LED_ERR_PINSEL_VAL 0xFFFCFFFF;
#define LED_ERR_IODIR IODIR0
#define LED_ERR_IOSET IOSET0
#define LED_ERR_IOCLR IOCLR0
#define LED_ERR_IOBIT 0x01000000

#endif
/*! @} */

/*! @defgroup msh_degauss_defines Magnetic Sensor Hybrid Degauss Pin Definitions
* @ingroup defines
* These constants define the IO port and specific bit to drive the MSH degauss pin.
* @{
*/
#define PAL_PINSEL PINSEL1
#define PAL_PINSEL_VAL 0xFFFCFFFF
#define PAL_IODIR IODIR0
#define PAL_IOSET IOSET0
#define PAL_IOCLR IOCLR0
#define PAL_IOBIT 0x00200000
/*! @} */

/*! @defgroup error_tresholds Error Treshold Constants
* @ingroup defines
* These constants define the tresholds of number of errors to generate an CAN error message.
* @{
*/
#define IMU_ERROR_TRESHOLD 1024
#define GPS_ERROR_TRESHOLD_RUN 2048
#define GPS_ERROR_TRESHOLD_INIT 30720
/*! @} */

/*! @defgroup error_codes Error Codes
* @ingroup defines
* Following constants define the error codes.
* @{
*/
#define NO_ERR 0x00
#define GENERAL_ERR 0x01
#define PARITY_ERR 0x02
#define RANGE_ERR 0x03
#define FRAME_ERR 0x04
#define FRAME_ERR_ETX 0x05
/*! @} */

/*! @defgroup can_id CAN Message IDs
* @ingroup defines
* The constants below define respective IDs for the CAN messages.
* Standard 11-bit IDs are used.
* @{
*/
#define RQ_CAN_ID 0x0014

#define RESET_REQUEST_ID 0x006E

#define FILTER_ROLL_A_ID 0x0070
#define FILTER_ROLL_B_ID 0x0071
#define FILTER_PITCH_A_ID 0x0072
#define FILTER_PITCH_B_ID 0x0073
#define FILTER_YAW_A_ID 0x0074
#define FILTER_YAW_B_ID 0x0075
#define FILTER_ACCEL_X_A_ID 0x0076
#define FILTER_ACCEL_X_B_ID 0x0077
#define FILTER_ACCEL_Y_A_ID 0x0078
#define FILTER_ACCEL_Y_B_ID 0x0079
#define FILTER_ACCEL_Z_A_ID 0x007A
#define FILTER_ACCEL_Z_B_ID 0x007B
#define CAL_SAMPLE_NUM_ID 0x0059

#define IMU_ERR_CAN_ID 0x0002
#define AR_CAN_ID_ROLL_FILTERED 0x0028
#define AR_CAN_ID_PITCH_FILTERED 0x0029
#define AR_CAN_ID_YAW_FILTERED 0x002A
#define AR_CAN_ID_ROLL 0x00C4
#define AR_CAN_ID_PITCH 0x01C4
#define AR_CAN_ID_YAW 0x03C4
#define AC_CAN_ID_X_FILTERED 0x0053
#define AC_CAN_ID_Y_FILTERED 0x0054
#define AC_CAN_ID_Z_FILTERED 0x0055
#define AC_CAN_ID_X 0x0056
#define AC_CAN_ID_Y 0x0057
#define AC_CAN_ID_Z 0x0058

#define GPS_ERR_CAN_ID 0x0004
#define GPS_ID1 0x0400
#define GPS_ID2 0x0200
#define GPS_ID3 0x0180
#define GPS_ID4 0x0100
#define GPS_ID5 0x0300
#define GPS_ID6 0x0500
#define GPS_ID7 0x0700
#define GPS_ID8 0x0080

#define MSH_ID_X 0x0020
#define MSH_ID_Y 0x0040
#define MSH_ID_Z 0x0060
/*! @} */

/*! @defgroup timer_settings Timer Settings
* @ingroup defines
* The constants below define the timer settings (timer frequency and interrupt number).
* @{
*/
/* 512 Hz */
#define TIMER_PRESCALER 1
#define TIMER_PERIOD 19531
#define ISR_INT_VEC_TIMER 10
/*! @} */

/*! @defgroup can_settings CAN Settings
* @ingroup defines
* The constants below define the CAN settings (baud rate and interrupt number).
* @{
*/
#define CAN_BTR 0x00250000
#define ISR_INT_VEC_CAN 11
/*! @} */

/*! @defgroup imu_constants IMU Communication Constants
* @ingroup defines
* The constants below define important IMU communication constants,
* such as message delimiter bytes and out-of-range bit mask.
* @{
*/
#define IMU_MES_DELIMITER_1 0x78
#define IMU_MES_DELIMITER_2 0x87
#define IMU_RANGE_MASK 0x3F
/*! @} */

/*! @defgroup gps_constants GPS Communication Constants
* @ingroup defines
* The constants below define important GPS communication constants,
* such as message header and delimiter bytes and length.
* @{
*/
#define DLE_BYTE 0x10
#define ETX_BYTE 0x03
#define ID_POS 0x33
#define GPS_DATA_LENGTH 0x40
/*! @} */

/*! @defgroup uart_settings UART Settings
* @ingroup defines
* The constants below define the UART settings, such as baud rates, interrupt numbers and buffer lengths.
* @{
*/
#define BAUD_RATE0 38400

/* a bit of dirty work - should be 9600, but 9530 better suits with respect to clock speed */
#define BAUD_RATE1 9530

#define ISR_INT_VEC_U0 12
#define ISR_INT_VEC_U1 13

#define BUFF_IMU_LEN 6
#define BUFF_GPS_LEN 64
/*! @} */

/*! @defgroup angular_rates_conversion Angular Rates Conversion Constant
* @ingroup defines
* @{
*/
/*!\def ANGULAR_CONV_64
* The constant below defines the angular rates conversion constant for 64Hz sampling rate.
*/
#define ANGULAR_CONV_64 0.000976563

/*!\def GYRO_OFFSET_FILTERING_TRESHOLD
* This constant defines the filter initialization treshold.
*/
#define GYRO_OFFSET_FILTERING_TRESHOLD 10
/*! @} */

/*! @defgroup accel_conversion Accelerations Conversion Constant
* @ingroup defines
* Accelerations conversion constants
* @{
*/
/*!\def ACCEL_CONV_64
* This constant is used to convert accelerations suppoted by the IMU from m/s2 into miligs for 64Hz sampling rate.
*/
/* ( 1/9.80665 ) * 64 */
#define ACCEL_CONV_64 6.5261838
/*! @} */

/*! @defgroup msh_conversion Magnetic Field Intensity Constants
* @ingroup defines
* @{
*/
/*!\def MSH_CONV
* AD value-to-mgauss conversion constant
*/
#define MSH_CONV 0.91

/*!\def MSH_OFFSET
* offset constant
*/
#define MSH_OFFSET 430

/*!\def BUFF_MSH_LEN
* Data buffer length
*/
#define BUFF_MSH_LEN 32
/*! @} */

/*! @defgroup filter_settings Filter Settings
* @ingroup defines
* The constants below define the polynomial filter coefficients for the roll, pitch and yaw angular rate filters.
* @{
*/
#define MAX_FILTER_LEN 6

#define FILTER_ROLL_A_NUM_FLAG 0x0001
#define FILTER_ROLL_B_NUM_FLAG 0x0002
#define FILTER_PITCH_A_NUM_FLAG 0x0004
#define FILTER_PITCH_B_NUM_FLAG 0x0008
#define FILTER_YAW_A_NUM_FLAG 0x0010
#define FILTER_YAW_B_NUM_FLAG 0x0020
#define FILTER_ACCEL_X_A_NUM_FLAG 0x0040
#define FILTER_ACCEL_X_B_NUM_FLAG 0x0080
#define FILTER_ACCEL_Y_A_NUM_FLAG 0x0100
#define FILTER_ACCEL_Y_B_NUM_FLAG 0x0200
#define FILTER_ACCEL_Z_A_NUM_FLAG 0x0400
#define FILTER_ACCEL_Z_B_NUM_FLAG 0x0800

#define FILTER_A_NUM 3
#define FILTER_B_NUM 3

#define FILTER_COEF_A_1 1.0
#define FILTER_COEF_A_2 0.462938
#define FILTER_COEF_A_3 0.2097154
#define FILTER_COEF_B_1 0.4181633
#define FILTER_COEF_B_2 0.836327
#define FILTER_COEF_B_3 0.4181633
/*! @} */
/*! @} */

uint8_t reset_req = 0; //!< reset request flag

uint16_t imu_timeout_count = 0; //!< IMU timeout counter
uint16_t gps_timeout_count = 0; //!< GPS timeout counter
uint16_t gps_err_treshold = GPS_ERROR_TRESHOLD_INIT; //!< GPS error treshold value (when surpassed, error message is generated)
uint8_t imu_err_state = 0; //!< IMU error status flag
uint8_t gps_err_state = 0; //!< GPS error status flag

uint8_t filter_roll_a_num = FILTER_A_NUM; //!< roll filter number of a coefficients
uint8_t filter_roll_b_num = FILTER_B_NUM; //!< roll filter number of b coefficients
uint8_t filter_pitch_a_num = FILTER_A_NUM; //!< pitch filter number of a coefficients
uint8_t filter_pitch_b_num = FILTER_B_NUM; //!< pitch filter number of b coefficients
uint8_t filter_yaw_a_num = FILTER_A_NUM; //!< yaw filter number of a coefficients
uint8_t filter_yaw_b_num = FILTER_B_NUM; //!< yaw filter number of b coefficients
uint8_t filter_accel_x_a_num = FILTER_A_NUM; //!< x-axis acceleration filter number of a coefficients
uint8_t filter_accel_x_b_num = FILTER_B_NUM; //!< x-axis acceleration filter number of b coefficients
uint8_t filter_accel_y_a_num = FILTER_A_NUM; //!< y-axis acceleration filter number of a coefficients
uint8_t filter_accel_y_b_num = FILTER_B_NUM; //!< y-axis acceleration filter number of b coefficients
uint8_t filter_accel_z_a_num = FILTER_A_NUM; //!< z-axis acceleration filter number of a coefficients
uint8_t filter_accel_z_b_num = FILTER_B_NUM; //!< z-axis acceleration filter number of b coefficients

uint8_t msh_meas_count = 0; //!< MSH sample counter
uint8_t msh_degauss_count = 0;  //!< MSH degauss (time) counter
int32_t mg_meas_x[BUFF_MSH_LEN]; //!< x-axis magnetic field intensity sample buffer
int32_t mg_meas_y[BUFF_MSH_LEN]; //!< y-axis magnetic field intensity sample buffer
int32_t mg_meas_z[BUFF_MSH_LEN]; //!< z-axis magnetic field intensity sample buffer

uint16_t cal_sample_num; //!< number of IMU calibration samples

double a_roll[MAX_FILTER_LEN] = {FILTER_COEF_A_1, FILTER_COEF_A_2, FILTER_COEF_A_3, 0, 0, 0}, b_roll[MAX_FILTER_LEN] = {FILTER_COEF_B_1, FILTER_COEF_B_2, FILTER_COEF_B_3, 0, 0, 0};  //!< roll filter coeffitients
double a_pitch[MAX_FILTER_LEN] = {FILTER_COEF_A_1, FILTER_COEF_A_2, FILTER_COEF_A_3, 0, 0, 0}, b_pitch[MAX_FILTER_LEN] = {FILTER_COEF_B_1, FILTER_COEF_B_2, FILTER_COEF_B_3, 0, 0, 0};  //!< pitch filter coeffitients
double a_yaw[MAX_FILTER_LEN] = {FILTER_COEF_A_1, FILTER_COEF_A_2, FILTER_COEF_A_3, 0, 0, 0}, b_yaw[MAX_FILTER_LEN] = {FILTER_COEF_B_1, FILTER_COEF_B_2, FILTER_COEF_B_3, 0, 0, 0};  //!< yaw filter coeffitients
double a_accel_x[MAX_FILTER_LEN] = {FILTER_COEF_A_1, FILTER_COEF_A_2, FILTER_COEF_A_3, 0, 0, 0}, b_accel_x[MAX_FILTER_LEN] = {FILTER_COEF_B_1, FILTER_COEF_B_2, FILTER_COEF_B_3, 0, 0, 0};  //!< x-axis acceleration filter coeffitients
double a_accel_y[MAX_FILTER_LEN] = {FILTER_COEF_A_1, FILTER_COEF_A_2, FILTER_COEF_A_3, 0, 0, 0}, b_accel_y[MAX_FILTER_LEN] = {FILTER_COEF_B_1, FILTER_COEF_B_2, FILTER_COEF_B_3, 0, 0, 0};  //!< y-axis acceleration filter coeffitients
double a_accel_z[MAX_FILTER_LEN] = {FILTER_COEF_A_1, FILTER_COEF_A_2, FILTER_COEF_A_3, 0, 0, 0}, b_accel_z[MAX_FILTER_LEN] = {FILTER_COEF_B_1, FILTER_COEF_B_2, FILTER_COEF_B_3, 0, 0, 0};  //!< z-axis acceleration filter coeffitients

can_msg_t imu_msg_err = {.id = IMU_ERR_CAN_ID}; //!< IMU CAN error message
can_msg_t gps_msg_err = {.id = GPS_ERR_CAN_ID}; //!< GPS CAN error message

/*! Timer 0 interrupt handler prototype. */
void timer_irq_handler ( void ) __attribute__ ( ( interrupt ) );

/*! Timer 0 initialization routine.
* Takes three arguments:
* \param prescale unsigned 32-bit int timer prescaler value
* \param period unsigned 32-bit int timer period value (counter reset occurs on match)
* \param irq_vect unsigned int timer interrupt number
*/
inline void timer_init ( uint32_t prescale, uint32_t period, unsigned irq_vect ) {
        T0PR = prescale - 1; // set prescaler
        T0MR1 = period - 1; // set period
        T0MCR = 0x0003 << 3; // interrupt and counter reset on match1
        T0EMR = 0x0001 << 6 | 0x2; // set MAT0.1 low on match and high now
        T0CCR = 0x0000; // no capture
        T0TCR |= 0x0001; // go!
        
        ( (uint32_t*) &VICVectAddr0 )[irq_vect] = (uint32_t) timer_irq_handler; // set the interrupt vector
        ( (uint32_t*) &VICVectCntl0 )[irq_vect] = 0x20 | 4;
        VICIntEnable = 0x00000010; // enable the timer 0 interrupt
}

/*! AD convertor initialization function.
* Takes one argument:
* \param clk_div unsigned 32-bit int timer prescaler value. Resulting clock speed should be less then or equal to 4.5MHz
*/
inline void adc_init ( uint32_t clk_div ) {
        ADCR = ( ( clk_div & 0x000000FF ) << 8 ) | 0x00200000; // set the clock prescaler and switch the AD convertor on
}

/*! Watchdog initialization function.
* Takes one argument:
* \param init_val unsigned 32-bit int timer initial value.
*/
inline void watchdog_init ( uint32_t init_val ) {
        WDTC = init_val; // timer counter start value
        WDMOD |= 0x03; // enable watchdog and reset capability
        WDFEED = 0xAA;
        WDFEED = 0x55; // feed sequence
}

/*! Watchdog reset function.
* Takes no arguments.
*/
inline void watchdog_reset ( void ) {
        WDFEED = 0xAA;
        WDFEED = 0x55; // feed sequence
}

/*! Buttleworth filter algorithm.
* Takes seven arguments:
* \param sample double new sample
* \param x_array double pointer to the filter denominator array
* \param y_array double pointer to the filter nominator array
* \param a_coef double pointer to the filter denominator coefficients  array
* \param b_coef double pointer to the filter nominator coefficients  array
* \param a_num unsigned 8-bit integer number of a coefficients
* \param b_num unsigned 8-bit integer number of b coefficients
*/
void filter ( double sample, double *x_array, double *y_array, double *a_coef, double *b_coef, uint8_t a_num, uint8_t b_num ) {
        uint8_t i;
        
        for ( i = 0; i < ( b_num - 1 ); i++ ) x_array[i] = x_array[i + 1]; // input data buffer shifting
        x_array[b_num - 1] = sample; // store new samle into the buffer
        
        for ( i = 0; i < ( a_num - 1 ); i++ ) y_array[i] = y_array[i + 1]; // output data buffer shifting
        y_array[a_num - 1] = 0; // store 0 at the end
        
        for ( i = 0; i < b_num; i++ ) y_array[a_num - 1] += b_coef[i] * x_array[b_num - i - 1]; // input polynom computation
        
        for ( i = 1; i < a_num; i++ ) y_array[a_num - 1] -= a_coef[i] * y_array[a_num - i - 1]; // input polynom computation
}

/*! Function sending a double number via CAN.
* Takes two arguments:
* \param id unsigned 32-bit integer message ID
* \param num double number to be sent
*/
void send_double_can ( uint32_t id, double num ) {
        can_msg_t msg = {.dlc = 8}; // CAN message
        
        /* decompose double into 8-bit fields (quartet byte order change) */
        memcpy ( &msg.data[4], &num, 4 );
        memcpy ( &msg.data[0], ( ( ( uint8_t * ) &num ) + 4 ), 4 );
        
        msg.id = id; // set the message ID
        while ( can_tx_msg ( &msg ) ); // send the CAN message
}

/*! Function initializing AD conversion and sampling the data.
* Takes one argument:
* \param channel AD channel to sample
* \return Returns sampled value.
*/
int32_t read_AD ( int32_t channel ) {
        int32_t read; // AD read value
        
        ADCR &= 0xFFFFFF00;
        ADCR |= channel; // initialize AD channel sample
        ADCR |= 0x01000000;
        while ( ! ( ( read = ADDR ) & 0x80000000 ) ); // wait for sample acquisition
        read &= 0x0000FFC0;
        read >>= 6; // read and preprocess sample
        
        return read;
}


/*! Timer 0 interrupt handler.
* Serves to handle response timeouts of IMU and GPS.
*/
void timer_irq_handler ( void ) {
        static uint16_t blinki = 0; // counter for LED blinking
        
        if ( !reset_req ) watchdog_reset ( ); // watchdog reset
        
        mg_meas_x[msh_meas_count] = read_AD ( 0x00000001 ); // store the result into respective buffer
        mg_meas_y[msh_meas_count] = read_AD ( 0x00000002 ); // store the result into respective buffer
        mg_meas_z[msh_meas_count] = read_AD ( 0x00000004 ); // store the result into respective buffer
        
        if ( msh_meas_count < BUFF_MSH_LEN ) msh_meas_count++; // increment the MSH sample counter
        
        if ( imu_timeout_count++ > IMU_ERROR_TRESHOLD ) { // if IMU is not responding for the preset time
                imu_err_state = 1;
                LED_IMU_IOCLR |= LED_IMU_IOBIT; // switch the IMU LED off
                LED_ERR_IOSET |= LED_ERR_IOBIT; // switch the ERR LED on
                imu_timeout_count = 0; // reset the IMU time counter
                imu_msg_err.data[imu_msg_err.dlc++] = GENERAL_ERR; // fill in the error code into the IMU error message structure
                while ( can_tx_msg ( &imu_msg_err ) ); // send the error message
                imu_msg_err.dlc = 0; // null the error message
        }
        
        if ( gps_timeout_count++ > gps_err_treshold ) { // if GPS is not responding for the preset time
                gps_err_state = 1;
                LED_GPS_IOCLR |= LED_GPS_IOBIT; // switch the GPS LED off
                LED_ERR_IOSET |= LED_ERR_IOBIT; // switch the ERR LED on
                gps_timeout_count = 0; // reset the GPS time counter
                gps_msg_err.data[gps_msg_err.dlc++] = GENERAL_ERR; // fill in the error code into the IMU error message structure
                while ( can_tx_msg ( &gps_msg_err ) ); // send the error message
                gps_msg_err.dlc = 0; // null the error message
        }
        
        if ( blinki++ < 128 ) LED_MSH_IOSET |= LED_MSH_IOBIT; // switch the MSH LED on for 0.25s
        else LED_MSH_IOCLR |= LED_MSH_IOBIT; // switch the MSH LED off for 0.25s
        if ( blinki > 256 ) blinki = 0; // after 0.5s, reset the LED blinking counter
        
        T0IR = 0xFFFFFFFF; // clear the interrupt flag
        VICVectAddr = 0; // int acknowledge
}

/*! Function adding coeffitients into the filter coeffitients array.
* Takes five arguments:
* \param filter_coef_flag unsigned 16-bit integer pointer filter coeffitient flag
* \param current_coef_flag unsigned 16-bit integer flag of the filter coeffitient currently being processed
* \param coef_num unsigned 8-bit integer pointer number of current coeffitient
* \param filter_array double pointer filter coeffitient array to which the coeffitient will be added
* \param rx_msg can_msg_t CAN message structure containing the coeffitient
*/
void filter_coef_add ( uint16_t *filter_coef_flag, uint16_t current_coef_flag, uint8_t *coef_num, double *filter_array, can_msg_t *rx_msg ) {
        if ( !( (*filter_coef_flag) & current_coef_flag ) ) { // if first filter ceficient received
                (*filter_coef_flag) |= current_coef_flag; // set the corresponding flag
                *coef_num = 0; // and null respective coeficient number
        }
        
        if ( *coef_num < MAX_FILTER_LEN ) {
                memcpy ( &filter_array[*coef_num], &(*rx_msg).data[4], 4 ); // compose double from 8-bit fields, store new coeficient
                memcpy ( ( ( ( uint8_t * ) &filter_array[(*coef_num)++] ) + 4 ), &(*rx_msg).data[0], 4 ); // and increment the coeficients counter
        }
}

/*! CAN Rx message process routine.
* Serves to handle an incoming CAN message.
*/
void can_rx ( void ) {
        typedef struct { // structure holding non-constant filter_coef_add function params
                uint16_t current_coef_flag;
                uint8_t *coef_num;
                double *filter_array;
        } filter_param_struct;
        
        filter_param_struct filter_param[12] = { {FILTER_ROLL_A_NUM_FLAG, &filter_roll_a_num, a_roll}, {FILTER_ROLL_B_NUM_FLAG, &filter_roll_b_num, b_roll}, {FILTER_PITCH_A_NUM_FLAG, &filter_pitch_a_num, a_pitch}, {FILTER_PITCH_B_NUM_FLAG, &filter_pitch_b_num, b_pitch}, {FILTER_YAW_A_NUM_FLAG, &filter_yaw_a_num, a_yaw}, {FILTER_YAW_B_NUM_FLAG, &filter_yaw_b_num, b_yaw}, {FILTER_ACCEL_X_A_NUM_FLAG, &filter_accel_x_a_num, a_accel_x}, {FILTER_ACCEL_X_B_NUM_FLAG, &filter_accel_x_b_num, b_accel_x}, {FILTER_ACCEL_Y_A_NUM_FLAG, &filter_accel_y_a_num, a_accel_y}, {FILTER_ACCEL_Y_B_NUM_FLAG, &filter_accel_y_b_num, b_accel_y}, {FILTER_ACCEL_Z_A_NUM_FLAG, &filter_accel_z_a_num, a_accel_z}, {FILTER_ACCEL_Z_B_NUM_FLAG, &filter_accel_z_b_num, b_accel_z} }; // array of structures holding non-constant filter_coef_add function params for various filters
        
        static uint16_t filter_coef_flag = 0; // filter coefitients reception flag
        can_msg_t rx_msg; // received message
        
        memcpy ( &rx_msg, (void *) &can_rx_msg, sizeof ( can_msg_t ) ); // create a local copy of the received message
        
        switch ( rx_msg.id ) { // switch according CAN message ID
                case RESET_REQUEST_ID: // when reset request is received
                        reset_req = TRUE; // set the reset request flag
                        while ( TRUE ); // wait for watchdog reset
                        break;
                
                case CAL_SAMPLE_NUM_ID: // receive number of calibration samples
                        memcpy ( &cal_sample_num, &rx_msg.data[0], sizeof ( uint16_t ) ); // compose uint16 from 8-bit fields
                        break;
                
                default: // receive filter coeficients
                        if ( ( rx_msg.id >= FILTER_ROLL_A_ID ) && ( rx_msg.id <= FILTER_ACCEL_Z_B_ID ) ) filter_coef_add ( &filter_coef_flag, filter_param[rx_msg.id - FILTER_ROLL_A_ID].current_coef_flag, filter_param[rx_msg.id - FILTER_ROLL_A_ID].coef_num, filter_param[rx_msg.id - FILTER_ROLL_A_ID].filter_array, &rx_msg ); // if filter coeffitient is received, add it to the respective filter coeffitients array
                        break;
        }
        
        can_msg_received = 0; // reset the pending CAN message flag
}

/*! IMU service routine.
* Serves to receive and process the IMU datagrams.
* \param *step unsigned 8-bit int pointer referencing the message processing step.
*/
void IMU_service ( uint8_t *step ) {
        uint8_t count, temp = 0; // auxiliary variables for wait cycle
        static uint8_t check_imu; // IMU datagram cross-parity computation variable
        static uint8_t c; // Auxiliary index variable
        static uint8_t sample_count = 0; // IMU sample counter
        static uint8_t blinki = 0; // counter variable driving the LED blinking
        static uint8_t buffer_int16[2]; // receive buffer for a 16-bit integer
        
        static int16_t buffer_imu[BUFF_IMU_LEN] = {0, 0, 0, 0, 0, 0}; // IMU data buffers
        
        static uint16_t cal_sample_count = 0;  // calibration sample counter
        
        int32_t msh_read_x; // MSH measurement sum x
        int32_t msh_read_y; // MSH measurement sum y
        int32_t msh_read_z; // MSH measurement sum z
        uint8_t msh_meas_count_local; // local copy of msh_meas_count
        double msh_val; // MSH sample
        
        double roll_compensated = 0; // Computed roll rate [64 Hz] with offset compensation
        double pitch_compensated = 0; // Computed pitch rate [64 Hz] with offset compensation
        double yaw_compensated = 0; // Computed yaw rate [64 Hz] with offset compensation
        double accel_x; // Computed x-axis acceleration
        double accel_y; // Computed y-axis acceleration
        double accel_z; // Computed z-axis acceleration
        
        static double roll_offset = 0; // roll gyro offset
        static double pitch_offset = 0; // pitch gyro offset
        static double yaw_offset = 0; // yaw gyro offset
        
        static double x_roll[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}, y_roll[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}; // roll rate filtering buffer
        static double x_pitch[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}, y_pitch[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}; // pitch rate filtering buffer
        static double x_yaw[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}, y_yaw[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}; // yaw rate filtering buffer
        static double x_accel_x[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}, y_accel_x[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}; // x-axis acceleration filtering buffer
        static double x_accel_y[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}, y_accel_y[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}; // y-axis acceleration filtering buffer
        static double x_accel_z[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}, y_accel_z[MAX_FILTER_LEN] = {0, 0, 0, 0, 0, 0}; // z-axis acceleration filtering buffer
        
        can_msg_t msg_rq = {.dlc = 0, .id = RQ_CAN_ID}; // time synchronization CAN message
        
        switch ( *step ) {
                case 0: if ( read_UART_data ( UART0 ) == IMU_MES_DELIMITER_1 ) ( *step )++; // wait for the first telegram start character
                        break;
                
                case 1: if ( read_UART_data ( UART0 ) == IMU_MES_DELIMITER_2 ) { // wait for the second telegram start character
                                ( *step )++; // increment the IMU step counter
                                
                                if ( ++sample_count > 1 ) while (can_tx_msg(&msg_rq)); // when the second teleram is being received, send the
                                                // synchonisation message (IMU running at 64Hz, overall system sampling frequency is 32Hz)
                        } else ( *step ) = 0; // should another character be received, return to the sequence beginning
                        
                        break;
                
                case 2: if ( blinki++ < 32 ) LED_IMU_IOSET |= LED_IMU_IOBIT; // switch the IMU LED on for 0.5s
                        else LED_IMU_IOCLR |= LED_IMU_IOBIT; // switch the IMU LED off for 0.5s
                        if ( blinki > 64 ) blinki = 0; // after 1s, reset the LED blinking counter
                        
                        check_imu = IMU_MES_DELIMITER_1; // cross parity initialization
                        check_imu ^= IMU_MES_DELIMITER_2; // cross parity initialization
                        
                        c = read_UART_data ( UART0 ); // read the next telegram character
                        check_imu ^= c; // cross parity computation
                        
                        if ( c & IMU_RANGE_MASK ) { // when IMU range error is detected, fill in respective error code
                                imu_msg_err.data[imu_msg_err.dlc++] = RANGE_ERR; // into the IMU error message structure
                                while ( can_tx_msg ( &imu_msg_err ) ); // send the error message
                                imu_msg_err.dlc = 0; // null the error message
                        }
                        
                        c = 0; // reset c, as it will be used as a cycle counter in the next two steps
                        ( *step )++;
                        break;
                
                /* read the accelerations and angular velocities */
                case 3: buffer_int16[0] = read_UART_data ( UART0 ); // read the first part of a 16-bit integer
                        check_imu ^= buffer_int16[0]; // cross parity computation
                        ( *step )++;
                        break;
                
                case 4: buffer_int16[1] = read_UART_data ( UART0 ); // read the second part of a 16-bit integer
                        check_imu ^= buffer_int16[1]; // cross parity computation
                        memcpy ( &buffer_imu[c], buffer_int16, sizeof ( int16_t ) ); // read the second part of a 16-bit integer
                        
                        if ( ++c < 6 ) ( *step )--; else ( *step )++; // when all 6 values are acquired go ahead, or return to the previous step
                        break;
                
                case 5: c = read_UART_data ( UART0 ); // read the cross parity
                        check_imu ^= c; // this final XOR should give a zero if the parity was right
                        
                        if ( check_imu ) { // should the parity be wrong fill in the respective error code
                                imu_msg_err.data[imu_msg_err.dlc++] = PARITY_ERR; // into the IMU error message structure
                                while ( can_tx_msg ( &imu_msg_err ) ); // send the error message
                                imu_msg_err.dlc = 0; // null the error message
                        }
                        
                        /* data filtering - roll, pitch and yaw angular rates */
                        filter ( ( ( (double) buffer_imu[0] ) * -ANGULAR_CONV_64 ), x_roll, y_roll, a_roll, b_roll, filter_roll_a_num, filter_roll_b_num );
                        filter ( ( ( (double) buffer_imu[1] ) * -ANGULAR_CONV_64 ), x_pitch, y_pitch, a_pitch, b_pitch, filter_pitch_a_num, filter_pitch_b_num );
                        filter ( ( ( (double) buffer_imu[2] ) * ANGULAR_CONV_64 ), x_yaw, y_yaw, a_yaw, b_yaw, filter_yaw_a_num, filter_yaw_b_num );
                        
                        /* gyros offset computation (calibration) */
                        if ( ( cal_sample_count >= GYRO_OFFSET_FILTERING_TRESHOLD ) && ( cal_sample_count < cal_sample_num ) ) { // summarize (cal_sample_num - GYRO_OFFSET_FILTERING_TRESHOLD) samples
                                roll_offset += y_roll[filter_roll_a_num - 1];
                                pitch_offset += y_pitch[filter_pitch_a_num - 1];
                                yaw_offset += y_yaw[filter_yaw_a_num - 1];
                                
                                /* ERR LED blinking */
                                if ( blinki < 32 ) LED_ERR_IOSET |= LED_ERR_IOBIT; // switch the ERR LED on for 0.5s
                                else LED_ERR_IOCLR |= LED_ERR_IOBIT; // switch the ERR LED off for 0.5s
                        } else if ( cal_sample_count == cal_sample_num ) { // compute offset mean values
                                roll_offset /= ( (double) ( cal_sample_num - GYRO_OFFSET_FILTERING_TRESHOLD ) );
                                pitch_offset /= ( (double) ( cal_sample_num - GYRO_OFFSET_FILTERING_TRESHOLD ) );
                                yaw_offset /= ( (double) ( cal_sample_num - GYRO_OFFSET_FILTERING_TRESHOLD ) );
                                
                                if ( !imu_err_state && !gps_err_state ) LED_ERR_IOCLR |= LED_ERR_IOBIT; // if IMU and GPS are not in the error state switch the ERR LED off
                                else LED_ERR_IOSET |= LED_ERR_IOBIT; // else if IMU or GPS (or both) are in the error state switch the ERR LED on
                        }
                        
                        /* filtered roll, pitch and yaw offset compensation */
                        if ( cal_sample_count > cal_sample_num ) {
                                roll_compensated = y_roll[filter_roll_a_num - 1] - roll_offset;
                                pitch_compensated = y_pitch[filter_pitch_a_num - 1] - pitch_offset;
                                yaw_compensated = y_yaw[filter_yaw_a_num - 1] - yaw_offset;
                        }
                        
                        if ( cal_sample_count < 0xFFFF ) cal_sample_count++; // increment calibration sample counter
                        
                        /* x, y and z acceleration computation */
                        accel_x = ( (double) buffer_imu[4] ) * ACCEL_CONV_64; // convert x-axis acceleration into miligs
                        accel_y = ( (double) buffer_imu[3] ) * ACCEL_CONV_64; // convert y-axis acceleration into miligs
                        accel_z = ( (double) buffer_imu[5] ) * -ACCEL_CONV_64; // convert z-axis acceleration into miligs (*(-1) due to the IMU orientation)
                        
                        /* data filtering - x, y and z axes accelerations */
                        filter ( accel_x, x_accel_x, y_accel_x, a_accel_x, b_accel_x, filter_accel_x_a_num, filter_accel_x_b_num );
                        filter ( accel_y, x_accel_y, y_accel_y, a_accel_y, b_accel_y, filter_accel_y_a_num, filter_accel_y_b_num );
                        filter ( accel_z, x_accel_z, y_accel_z, a_accel_z, b_accel_z, filter_accel_z_a_num, filter_accel_z_b_num );
                        
                        if ( sample_count > 1 ) { // if the second telegram is received, process the data
                                send_double_can ( AR_CAN_ID_ROLL_FILTERED, roll_compensated ); // send the CAN message containing filtered roll angular velocity
                                send_double_can ( AR_CAN_ID_PITCH_FILTERED, pitch_compensated ); // send the CAN message containing filtered pitch angular velocity
                                send_double_can ( AR_CAN_ID_YAW_FILTERED, yaw_compensated ); // send the CAN message containing filtered yaw angular velocity
                                send_double_can ( AR_CAN_ID_ROLL, ( ( (double) buffer_imu[0] ) * -ANGULAR_CONV_64 ) ); // send the CAN message containing roll angular velocity
                                send_double_can ( AR_CAN_ID_PITCH, ( ( (double) buffer_imu[1] ) * -ANGULAR_CONV_64 ) ); // send the CAN message containing pitch angular velocity
                                send_double_can ( AR_CAN_ID_YAW, ( ( (double) buffer_imu[2] ) * ANGULAR_CONV_64 ) ); // send the CAN message containing yaw angular velocity
                                send_double_can ( AC_CAN_ID_X_FILTERED, y_accel_x[filter_accel_x_a_num - 1] ); // send the CAN message containing filtered x-axis acceleration
                                send_double_can ( AC_CAN_ID_Y_FILTERED, y_accel_y[filter_accel_y_a_num - 1] ); // send the CAN message containing filtered y-axis acceleration
                                send_double_can ( AC_CAN_ID_Z_FILTERED, y_accel_z[filter_accel_z_a_num - 1] ); // send the CAN message containing filtered z-axis acceleration
                                send_double_can ( AC_CAN_ID_X, accel_x ); // send the CAN message containing x-axis acceleration
                                send_double_can ( AC_CAN_ID_Y, accel_y ); // send the CAN message containing y-axis acceleration
                                send_double_can ( AC_CAN_ID_Z, accel_z ); // send the CAN message containing z-axis acceleration
                                
                                /* MSH messages composition */
                                msh_read_x = 0; // clear the MSH measure sum x
                                msh_read_y = 0; // clear the MSH measure sum y
                                msh_read_z = 0; // clear the MSH measure sum z
                                msh_meas_count_local = msh_meas_count; // store msh measure counter into a local copy
                                for ( c = 0; c < msh_meas_count_local; c++ ) {
                                        msh_read_x += mg_meas_x[c];
                                        msh_read_y += mg_meas_y[c]; // compute the sums
                                        msh_read_z += mg_meas_z[c];
                                }
                                
                                msh_meas_count = 0; // reset the MSH measure counter
                                
                                if ( msh_degauss_count++ > 127 ) { // generate the degauss pulse for MSH every 4s
                                        PAL_IOCLR |= PAL_IOBIT; // degauss pulse (+)
                                        for ( count = 0; count < 60; count++ ) temp++; // delay between pulses
                                        PAL_IOSET |= PAL_IOBIT; // degauss pulse (-)
                                        msh_degauss_count = 0; // reset degauss timer counter
                                }
                                
                                msh_val = ( ( double ) msh_read_x ) / ( ( double ) msh_meas_count_local ); // mean value computation x
                                msh_val /= MSH_CONV; // conversion to mgauss
                                msh_val -= MSH_OFFSET; // offset correction
                                msh_val = -msh_val; // reverse direction to correspond with the vehicle axis orientation
                                
                                send_double_can ( MSH_ID_X, msh_val ); // send the CAN message (y magnetic field intensity)
                                
                                msh_val = ( ( double ) msh_read_y ) / ( ( double ) msh_meas_count_local ); // mean value computation y
                                msh_val /= MSH_CONV; // conversion to mgauss
                                msh_val -= MSH_OFFSET; // offset correction
                                
                                send_double_can ( MSH_ID_Y, msh_val ); // send the CAN message (y magnetic field intensity)
                                
                                msh_val = ( ( double ) msh_read_z ) / ( ( double ) msh_meas_count_local ); // mean value computation z
                                msh_val /= MSH_CONV; // conversion to mgauss
                                msh_val -= MSH_OFFSET; // offset correction
                                
                                send_double_can ( MSH_ID_Z, msh_val ); // send the CAN message (z magnetic field intensity)
                                
                                imu_timeout_count = 0; // reset the IMU timeout counter
                                sample_count = 0; // reset the sample counter
                                
                                if ( imu_err_state ) { // if IMU was in error state
                                        imu_msg_err.data[imu_msg_err.dlc++] = NO_ERR; // fill in the no error code into the IMU error message structure
                                        while ( can_tx_msg ( &imu_msg_err ) ); // send the error message
                                        imu_msg_err.dlc = 0; // null the error message
                                        LED_ERR_IOCLR |= LED_ERR_IOBIT; // switch the ERR LED off
                                        imu_err_state = 0; // turn off the GPS error flag
                                }
                        }
                        
                        ( *step ) = 0; // repeat the cycle from the beginning
                        break;
        }
}

/*! GPS service routine
* Serves to receive and process the GPS datagrams.
* \param *step unsigned 8-bit int pointer referencing the message processing step.
*/
void GPS_service ( uint8_t *step ) {
        uint32_t gps_can_msg_id[8] = {GPS_ID1, GPS_ID2, GPS_ID3, GPS_ID4, GPS_ID5, GPS_ID6, GPS_ID7, GPS_ID8}; // GPS can messages ID array
        uint8_t i; // cycle counter
        static uint8_t check_gps; // GPS datagram cross-parity computation variable
        static uint8_t gps_data_count; // GPS datagram received byte counter
        static uint8_t dle_data_flag = 0; // Flag variable, signalizing that bit stuffing occured
        static uint8_t blinki = 0; // counter variable driving the LED blinking
        static uint8_t buffer_gps[BUFF_GPS_LEN]; // GPS receive buffer
        
        can_msg_t gps_msg = {.dlc = 8}; // GPS CAN data packet
        
        switch ( *step ) {
                case 0: if ( read_UART_data ( UART1 ) == DLE_BYTE ) ( *step )++; // wait for the first telegram start character
                        break;
                
                case 1: if ( read_UART_data ( UART1 ) == ID_POS ) { // check for the position telegram ID
                                check_gps = ID_POS; // cross parity computation (sum of all telegram characters should be 0)
                                ( *step )++; // go to the next step
                        } else ( *step ) = 0; // if other then position telegram ID is detected, go to the beginning and wait for another telegram
                        
                        break;
                
                case 2: if ( read_UART_data ( UART1 ) == GPS_DATA_LENGTH ) { // check the data message length
                                check_gps += GPS_DATA_LENGTH; // cross parity computation (sum of all telegram characters should be 0)
                                
                                if ( blinki++ < 1 ) LED_GPS_IOSET |= LED_GPS_IOBIT; // switch the GPS LED on for 1s
                                else LED_GPS_IOCLR |= LED_GPS_IOBIT; // switch the GPS LED off for 1s
                                if ( blinki > 1 ) blinki = 0; // after 2s, reset the LED blinking counter
                                
                                gps_data_count = 0; // reset the GPS Rx data counter
                                ( *step )++; // go to the next step
                        } else ( *step ) = 0; // if wrong data length is detected, go to the beginning and wait for another telegram
                        
                        break;
                
                case 3: buffer_gps[gps_data_count++] = read_UART_data ( UART1 ); // store new GPS data into the GPS data buffer
                        
                        if ( ( buffer_gps[gps_data_count - 1] == DLE_BYTE ) && !dle_data_flag ) { // get rid of the DLE byte stuffing
                                gps_data_count--; // shift the data counter back - the stuffed character should be omitted
                                dle_data_flag = 1; // set the DLE stuffing flag
                        } else if ( dle_data_flag ) dle_data_flag = 0; // in the next step, clear the DLE stuffing flag
                        
                        if ( !dle_data_flag ) check_gps += buffer_gps[gps_data_count - 1]; // cross parity computation (sum of all telegram characters should be 0)
                        
                        if ( gps_data_count == GPS_DATA_LENGTH ) ( *step )++; // if the whole message has been read go to the next step
                        break;
                
                case 4: dle_data_flag = read_UART_data ( UART1 ); // read the last parity byte
                        check_gps += dle_data_flag; // cross parity computation
                        if ( check_gps ) { // if the parity is wrong fill in the respective error code
                                gps_msg_err.data[gps_msg_err.dlc++] = PARITY_ERR; // into the IMU error message structure
                                while ( can_tx_msg ( &gps_msg_err ) ); // send the error message
                                gps_msg_err.dlc = 0; // null the error message
                        }
                        
                        ( *step )++; // follow to the next step
                        break;
                
                case 5: if ( read_UART_data ( UART1 ) != DLE_BYTE ) { // read the second DLE byte (DLE stuffing). In case it is not received,
                                gps_msg_err.data[gps_msg_err.dlc++] = FRAME_ERR; // fill the respective error code into the IMU error message structure
                                while ( can_tx_msg ( &gps_msg_err ) ); // send the error message
                                gps_msg_err.dlc = 0; // null the error message
                        }
                        
                        if ( dle_data_flag != DLE_BYTE ) ( *step )++; else dle_data_flag = 0; // follow to the next step, if DLE stuffing occured repeat the step 5 once more
                        break;
                
                case 6: if ( read_UART_data ( UART1 ) != ETX_BYTE ) { // the ETX byte should follow...
                                gps_msg_err.data[gps_msg_err.dlc++] = FRAME_ERR_ETX; // in case it does not, fill in the respective error code into the IMU error message structure
                                while ( can_tx_msg ( &gps_msg_err ) ); // send the error message
                                gps_msg_err.dlc = 0; // null the error message
                        }
                        
                        ( *step )++; // follow to the next step
                        break;
                
                /* compose and send the CAN messages containing the GPS data */
                case 7: for ( i = 0; i < 8; i++ ) {
                                memcpy ( gps_msg.data, &buffer_gps[8 * i], 8 ); // fill in the first data message
                                gps_msg.id = gps_can_msg_id[i]; // set the ID
                                while ( can_tx_msg ( &gps_msg ) ); // send the GPS data packet
                        }
                        
                        gps_timeout_count = 0; // reset the GPS timeout counter
                        gps_err_treshold = GPS_ERROR_TRESHOLD_RUN;
                        ( *step ) = 0; // repeat the cycle from the beginning
                        
                        if ( gps_err_state ) { // if GPS was in error state
                                gps_msg_err.data[gps_msg_err.dlc++] = NO_ERR; // fill in the no error code into the GPS error message structure
                                while ( can_tx_msg ( &gps_msg_err ) ); // send the error message
                                gps_msg_err.dlc = 0; // null the error message
                                LED_ERR_IOCLR |= LED_ERR_IOBIT; // switch the ERR LED off
                                gps_err_state = 0; // turn off the GPS error flag
                        }
                        
                        break;
        }
}

/*! Application entry point.
* This is where the application starts. This routine consists of
* initialization and an infinite cycle, where pending data are detected and
* respective service routines called to process them.
* \return Returns nothing - the program is an infinite cycle.
*/
int main ( void ) {
        uint8_t step_imu = 0; // actual step number in the IMU datagram processing
        uint8_t step_gps = 0; // actual step number in the GPS datagram processing
        int count, temp = 0; // auxiliary variables for wait cycle
        
        VPBDIV = 1; // peripheral clock = CPU clock (10MHz)
        
        MEMMAP = MEMMAP_EXC_RAM; // map exception handling to RAM
        
        VICIntEnClr = 0xFFFFFFFF; // init Vector Interrupt Controller
        VICIntSelect = 0x00000000;
        
        PAL_PINSEL &= PAL_PINSEL_VAL;
        PAL_IODIR |= PAL_IOBIT; // set the PAL pin as output and set PAL to 1 (inactive)
        PAL_IOSET |= PAL_IOBIT;
        
        LED_IMU_PINSEL &= LED_IMU_PINSEL_VAL;
        LED_IMU_IODIR |= LED_IMU_IOBIT; // set the IMU LED pin as output and switch the IMU LED off
        LED_IMU_IOCLR |= LED_IMU_IOBIT;
        
        LED_GPS_PINSEL &= LED_GPS_PINSEL_VAL;
        LED_GPS_IODIR |= LED_GPS_IOBIT; // set the GPS LED pin as output and switch the GPS LED off
        LED_GPS_IOCLR |= LED_GPS_IOBIT;
        
        LED_MSH_PINSEL &= LED_MSH_PINSEL_VAL;
        LED_MSH_IODIR |= LED_MSH_IOBIT; // set the MSH LED pin as output and switch the MSH LED off
        LED_MSH_IOCLR |= LED_MSH_IOBIT;
        
        LED_ERR_PINSEL &= LED_ERR_PINSEL_VAL;
        LED_ERR_IODIR |= LED_ERR_IOBIT; // set the ERR LED pin as output and switch the ERR LED on
        LED_ERR_IOSET |= LED_ERR_IOBIT;
        
        pwm_channel ( 2, 0 ); // select PWM channel to be used as clock source for Villard
        pwm_init ( 1, 2000 ); // init PWM
        pwm_set ( 2, 1000 ); // switch on Villard 5kHz
        
        can_init ( CAN_BTR, ISR_INT_VEC_CAN, NULL ); // init CAN
        watchdog_init ( WATCHDOG_INIT ); // watchdog initialization
        
        for ( count = 0; count < 2500000; count++ ) { // wait for Villard to charge
                if ( can_msg_received ) can_rx ( ); // when CAN receives new data, call the service routine
                if ( WDTV < ( WATCHDOG_INIT / 2 ) ) watchdog_reset ( ); // watchdog reset
        }
        
        PAL_IOCLR |= PAL_IOBIT; // degauss pulse (+)
        for ( count = 0; count < 100; count++ ) temp++; // delay between pulses
        PAL_IOSET |= PAL_IOBIT; // degauss pulse (-)
        
        LED_ERR_IOCLR |= LED_ERR_IOBIT; // switch the ERR LED off
        
        adc_init ( 3 ); // init AD convertor (AD clock prescaller 3)
        UART_init ( UART0, BAUD_RATE0, ISR_INT_VEC_U0 ); // init UART0
        UART_init ( UART1, BAUD_RATE1, ISR_INT_VEC_U1 ); // init UART1
        timer_init ( TIMER_PRESCALER, TIMER_PERIOD, ISR_INT_VEC_TIMER ); // init timer 0 - interrupt frequency 1Hz
        
        /* main infinite cycle - look for new data at both UARTs and CAN */
        while ( TRUE ) {
                if ( UART_new_data ( UART0 ) ) IMU_service ( &step_imu ); // when UART0 receives new data, service the IMU
                if ( UART_new_data ( UART1 ) ) GPS_service ( &step_gps ); // when UART1 receives new data, service the GPS
                if ( can_msg_received ) can_rx ( ); // when CAN receives new data, call the service routine
        }
}