Adjusted status LED definitions

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

/********************************************************/
/*      Data Acqusition software for the DAM module     */
/*              RAMA UAV control system                 */
/*                                                      */
/*              ver. 5.6 beta                           */
/*                                                      */
/* (c) 2006-2009 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 <pll.h>
#include <periph/can.h>
#include <periph/uart_zen.h>
#include <pwm.h>

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

/*! @defgroup VERSION Program Version
* @ingroup defines
* These constants define the program version.
* @{
*/
#define MAIN_VERSION 5
#define SUB_VERSION 6
/*! @} */

/*! @defgroup CLK Clock settings
* @ingroup defines
* These constants define the crystal frequency, core clock and peripheral clock.
* @{
*/
#define OSC_FREQ 10000000
#define CCLK 60000000
#define PCLK 60000000
/*! @} */

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

/*!\def SAMPLING_FREQ_PRESCALER
* Sampling frequency prescaler - 64Hz / ( SAMPLING_FREQ_PRESCALER + 1 ), that means that 0 - 64Hz, 1 - 32Hz etc.
*/
/* 64Hz */
#define SAMPLING_FREQ_PRESCALER 0

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

/*! @defgroup led_defines Status LEDs Definitions
* @ingroup defines
* These constants define the IO ports and specific bits to manipulate respective status LEDs.
* @{
*/
#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
/*! @} */

/*! @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 3072
#define GPS_ERROR_TRESHOLD_RUN 6144
#define GPS_ERROR_TRESHOLD_INIT 65534
/*! @} */

/*! @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_ETX_ERR 0x05
#define UART_ROUND_BUFFER_OVERRUN_ERR 0x06
#define UART_RX_BUFFER_OVERRUN_ERR 0x07
#define UART_PARITY_ERR 0x08
#define UART_FRAME_ERR 0x09
#define UART_BREAK_IRQ_ERR 0x0A
/*! @} */

/*! @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 DAM_RESET_ID 0x0064
#define RESET_REQUEST_ID 0x006E
#define RUN_REQUEST_ID 0x006F

#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 FILTER_MSH_X_A_ID 0x007C
#define FILTER_MSH_X_B_ID 0x007D
#define FILTER_MSH_Y_A_ID 0x007E
#define FILTER_MSH_Y_B_ID 0x007F
#define FILTER_MSH_Z_A_ID 0x0080
#define FILTER_MSH_Z_B_ID 0x0081
#define MSH_OFFSET_X_ID 0x0082
#define MSH_OFFSET_Y_ID 0x0083
#define MSH_OFFSET_Z_ID 0x0084
#define CAL_SAMPLE_NUM_ID 0x0059

#define IMU_ERR_CAN_ID 0x0002
#define AR_CAN_ID_ROLL 0x00C4
#define AR_CAN_ID_PITCH 0x01C4
#define AR_CAN_ID_YAW 0x03C4
#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 0x0090

#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).
* @{
*/
/* 1536 Hz */
#define TIMER_PRESCALER 1
#define TIMER_PERIOD ( PCLK / 1536 )
#define ISR_INT_VEC_TIMER 10
/*! @} */

/*!\def ADC_PRESCALER
* The constant below defines the AD converter clock prescaler.
*/
#define ADC_PRESCALER 14

/*! @defgroup can_settings CAN Settings
* @ingroup defines
* The constants below define the CAN settings (baud rate and interrupt number).
* @{
*/
#define CAN_BTR ( 0x00250000 + ( PCLK / 10000000 ) - 1 )
#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
#define BAUD_RATE1 9600

#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
* The constant below defines the angular rates conversion constant for 64Hz sampling rate.
*/
#define ANGULAR_CONV 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
* 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 6.5261838
/*! @} */

/*! @defgroup msh_conversion Magnetic Field Intensity Constants
* @ingroup defines
* @{
*/
/*! @defgroup MSH_CONV AD value-to-mgauss conversion constants
* @ingroup msh_conversion
* @{
*/
#define MSH_X_CONV 1.14
#define MSH_Y_CONV 1.05
#define MSH_Z_CONV 1.05
/*! @} */

/*! @defgroup PWM_PARAMS PWM parameters (PWM is used for Villard charging)
* @ingroup msh_conversion
* @{
*/
/* 1KHz, 1/2 pulse ratio */
#define PWM_PERIOD 60000
#define PWM_PULSE_LENGTH 30000
/*! @} */

/*!\def VILLARD_CHARGE_WAIT
* Wait constant for initial Villard charge, used for magnetometer degaussing
*/
#define VILLARD_CHARGE_WAIT 6000000
/*! @} */

/*! @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 0x00000001
#define FILTER_ROLL_B_NUM_FLAG 0x00000002
#define FILTER_PITCH_A_NUM_FLAG 0x00000004
#define FILTER_PITCH_B_NUM_FLAG 0x00000008
#define FILTER_YAW_A_NUM_FLAG 0x00000010
#define FILTER_YAW_B_NUM_FLAG 0x00000020
#define FILTER_ACCEL_X_A_NUM_FLAG 0x00000040
#define FILTER_ACCEL_X_B_NUM_FLAG 0x00000080
#define FILTER_ACCEL_Y_A_NUM_FLAG 0x00000100
#define FILTER_ACCEL_Y_B_NUM_FLAG 0x00000200
#define FILTER_ACCEL_Z_A_NUM_FLAG 0x00000400
#define FILTER_ACCEL_Z_B_NUM_FLAG 0x00000800
#define FILTER_MSH_X_A_NUM_FLAG 0x00001000
#define FILTER_MSH_X_B_NUM_FLAG 0x00002000
#define FILTER_MSH_Y_A_NUM_FLAG 0x00004000
#define FILTER_MSH_Y_B_NUM_FLAG 0x00008000
#define FILTER_MSH_Z_A_NUM_FLAG 0x00010000
#define FILTER_MSH_Z_B_NUM_FLAG 0x00020000
/*! @} */

/*! @defgroup run_time_flags Run-Time Flags
* @ingroup defines
* The constants below define the run-time flags (used to skip the calibration sequence, initial degaussing and filter coeffitients reception or to signal reset request).
* @{
*/
#define COEF_RECEPTION_FINISH_FLAG 0x01
#define RUN_TIME_FLAG 0x02
#define RESET_REQUEST_FLAG 0x04
/*! @} */
/*! @} */

volatile uint8_t run = 0x00; //!< run-time flag variable (used to skip the calibration sequence, initial degaussing and filter coeffitients reception or to signal reset request)

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

uint8_t filter_roll_a_num = 0; //!< roll filter number of a coefficients
uint8_t filter_roll_b_num = 0; //!< roll filter number of b coefficients
uint8_t filter_pitch_a_num = 0; //!< pitch filter number of a coefficients
uint8_t filter_pitch_b_num = 0; //!< pitch filter number of b coefficients
uint8_t filter_yaw_a_num = 0; //!< yaw filter number of a coefficients
uint8_t filter_yaw_b_num = 0; //!< yaw filter number of b coefficients
uint8_t filter_accel_x_a_num = 0; //!< x-axis acceleration filter number of a coefficients
uint8_t filter_accel_x_b_num = 0; //!< x-axis acceleration filter number of b coefficients
uint8_t filter_accel_y_a_num = 0; //!< y-axis acceleration filter number of a coefficients
uint8_t filter_accel_y_b_num = 0; //!< y-axis acceleration filter number of b coefficients
uint8_t filter_accel_z_a_num = 0; //!< z-axis acceleration filter number of a coefficients
uint8_t filter_accel_z_b_num = 0; //!< z-axis acceleration filter number of b coefficients
uint8_t filter_msh_x_a_num = 0; //!< x-axis magnetometer filter number of a coefficients
uint8_t filter_msh_x_b_num = 0; //!< x-axis magnetometer filter number of b coefficients
uint8_t filter_msh_y_a_num = 0; //!< y-axis magnetometer filter number of a coefficients
uint8_t filter_msh_y_b_num = 0; //!< y-axis magnetometer filter number of b coefficients
uint8_t filter_msh_z_a_num = 0; //!< z-axis magnetometer filter number of a coefficients
uint8_t filter_msh_z_b_num = 0; //!< z-axis magnetometer filter number of b coefficients

float msh_val[3]; //!< computed magnetic field intensities in x, y and z axes
volatile uint8_t msh_sent_flag = 0;

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

float msh_offset[3] = {0.0, 0.0, 0.0}; // MSH offsets

float a_roll[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, b_roll[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  //!< roll filter coeffitients
float a_pitch[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, b_pitch[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  //!< pitch filter coeffitients
float a_yaw[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, b_yaw[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  //!< yaw filter coeffitients
float a_accel_x[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, b_accel_x[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  //!< x-axis acceleration filter coeffitients
float a_accel_y[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, b_accel_y[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  //!< y-axis acceleration filter coeffitients
float a_accel_z[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, b_accel_z[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  //!< z-axis acceleration filter coeffitients
float a_msh_x[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, b_msh_x[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  //!< x-axis magnetometer filter coeffitients
float a_msh_y[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, b_msh_y[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  //!< y-axis magnetometer filter coeffitients
float a_msh_z[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, b_msh_z[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  //!< z-axis magnetometer filter coeffitients

can_msg_t imu_msg_err = {.id = IMU_ERR_CAN_ID, .dlc = 1}; //!< IMU CAN error message
can_msg_t gps_msg_err = {.id = GPS_ERR_CAN_ID, .dlc = 1}; //!< 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 << 0x0003; // interrupt and counter reset on match1
        T0EMR = 0x0001 << 0x0006 | 0x0002; // 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 | 0x04;
        VICIntEnable = 0x00000010; // enable the timer 0 interrupt
}

/*! AD converter 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 = 0x00000004 | ( ( ( clk_div - 1 ) & 0x000000FF ) << 8 ) | 0x00200000; // set the clock prescaler and switch the AD converter on
        ADCR |= 0x04000000; // conversion started on falling edge of MAT0.1
}

/*! 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 float new sample
* \param x_array float pointer to the filter denominator array
* \param y_array float pointer to the filter nominator array
* \param a_coef float pointer to the filter denominator coefficients  array
* \param b_coef float 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 ( float sample, float *x_array, float *y_array, float *a_coef, float *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.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 float number via CAN.
* Takes two arguments:
* \param id unsigned 32-bit integer message ID
* \param num1 float pointer to number 1 to be sent (must not be NULL - no error checking for simplicity)
* \param num2 float pointer to number 2 to be sent (may be NULL)
*/
void send_float_can ( uint32_t id, float *num1, float *num2 ) {
        can_msg_t msg = {.dlc = 4}; // CAN message
        
        /* decompose float into bytes */
        memcpy ( msg.data, num1, sizeof ( float ) );
        
        if ( num2 != NULL ) {
                memcpy ( &msg.data[4], num2, sizeof ( float ) );
                msg.dlc += sizeof ( float );
        }
        
        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 = 0; // AD read value
        
        while ( !( read & 0x80000000 ) ) read = ADDR; // wait for sample acquisition
        
        T0EMR |= 0x00000002; // set MAT0.1 high again
        read = ( read >> 6 ) & 0x000003FF; // read and preprocess sample
        ADCR = ( ADCR & 0xFFFFFF00 ) | channel; // select next AD channel to sample
        
        return read;
}

/*! Timer 0 interrupt handler.
* Serves to handle response timeouts of IMU and GPS.
*/
void timer_irq_handler ( void ) {
        uint32_t count; // cycle index
        static uint16_t blinki = 0; // counter for LED blinking
        static uint8_t msh_channel_count = 0; // msh channel counter (selecting AD channels for x, y and z magnetometers)
        static uint8_t msh_meas_count = 0; // MSH sample counter
        static int32_t msh_meas[3]; // x, y and z axes magnetic field intensity sample sum buffer
        static uint16_t msh_degauss_count = 0; // MSH degauss (timer) counter
        const float msh_conv[3] = {MSH_Y_CONV, MSH_Z_CONV, MSH_X_CONV}; // MSH conversion constants
        
        if ( !( run & RESET_REQUEST_FLAG ) ) watchdog_reset ( ); // watchdog reset
        
        if ( msh_degauss_count++ > 12288 ) { // if the MSH sensor was not degaussed for approx. 8s
                PAL_IOSET |= PAL_IOBIT; // degauss pulse (-)
                msh_degauss_count = 0; // reset MSH degauss counter
        }
        
        if ( msh_degauss_count == 7 ) PAL_IOCLR |= PAL_IOBIT; // degauss pulse (+) (approx. 4.5ms after the (-) pulse)
        
        if ( msh_degauss_count > 8 ) { // do not measure while degaussing
                msh_meas[msh_channel_count] += read_AD ( 0x00000001 << msh_channel_count ); // store the result into respective buffer
                
                if ( ++msh_channel_count > 2 ) { // if msh channel counter exceeds the number of channels (starting from 0)
                        msh_channel_count = 0; // reset the msh channel counter
                        msh_meas_count++; // increment the MSH sample counter
                        
                        for ( count = 0; count < 3; count++ ) { // compute magnetic field intensities using sampled MSH values
                                msh_val[count] = ( ( ( float ) msh_meas[count] ) / ( ( float ) msh_meas_count ) ); // mean value computation
                                msh_val[count] -= msh_offset[count]; // offset correction
                                msh_val[count] *= msh_conv[count]; // conversion to mgauss
                        }
                        
                        if ( msh_sent_flag ) { // if the MSH values had been sent
                                memset ( msh_meas, 0, 12 ); // reset MSH buffers
                                msh_meas_count = 0; // reset MSH sample counter
                                msh_sent_flag = 0; // resent MSH values sent flag
                        }
                }
        }
        
        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[0] = GENERAL_ERR; // fill in the error code into the IMU error message structure
                while ( can_tx_msg ( &imu_msg_err ) ); // send 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[0] = GENERAL_ERR; // fill in the error code into the IMU error message structure
                while ( can_tx_msg ( &gps_msg_err ) ); // send the error message
        }
        
        if ( blinki++ < 384 ) 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 > 768 ) 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 32-bit integer pointer filter coeffitient flag
* \param current_coef_flag unsigned 32-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 float 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 ( uint32_t *filter_coef_flag, uint32_t current_coef_flag, uint8_t *coef_num, float *filter_array, can_msg_t *rx_msg ) {
        if ( !( *filter_coef_flag & current_coef_flag ) ) { // if first filter coeficient 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, sizeof ( float ) ); // compose float from 4 bytes, store new coeficient
        }
}

/*! 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
                uint32_t current_coef_flag;
                uint8_t *coef_num;
                float *filter_array;
        } filter_param_struct;
        
        const filter_param_struct filter_param[18] = { {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}, {FILTER_MSH_X_A_NUM_FLAG, &filter_msh_x_a_num, a_msh_x}, {FILTER_MSH_X_B_NUM_FLAG, &filter_msh_x_b_num, b_msh_x}, {FILTER_MSH_Y_A_NUM_FLAG, &filter_msh_y_a_num, a_msh_y}, {FILTER_MSH_Y_B_NUM_FLAG, &filter_msh_y_b_num, b_msh_y}, {FILTER_MSH_Z_A_NUM_FLAG, &filter_msh_z_a_num, a_msh_z}, {FILTER_MSH_Z_B_NUM_FLAG, &filter_msh_z_b_num, b_msh_z} }; // array of structures holding filter_coef_add function params for various filters
        
        static uint32_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
                        run |= RESET_REQUEST_FLAG; // set the reset request flag
                        while ( TRUE ); // wait for watchdog reset
                
                case MSH_OFFSET_X_ID: // magnetometer x-axis offset
                        if ( !run ) memcpy ( &msh_offset[2], rx_msg.data, sizeof ( float ) ); // compose float from 4 bytes, store x-axis offset
                        break;
                
                case MSH_OFFSET_Y_ID: // magnetometer y-axis offset
                        if ( !run ) memcpy ( msh_offset, rx_msg.data, sizeof ( float ) ); // compose float from 4 bytes, store y-axis offset
                        break;
                
                case MSH_OFFSET_Z_ID: // magnetometer z-axis offset
                        if ( !run ) memcpy ( &msh_offset[1], rx_msg.data, sizeof ( float ) ); // compose float from 4 bytes, store z-axis offset
                        break;
                
                case RUN_REQUEST_ID: // request to skip calibration and initial degaussing and to prevent further filter coeffitients update
                        run |= RUN_TIME_FLAG; // set the run-time flag
                        break;
                
                case CAL_SAMPLE_NUM_ID: // receive number of calibration samples
                        if ( !run ) { // if no run-time flag is set
                                memcpy ( &cal_sample_num, rx_msg.data, sizeof ( uint16_t ) ); // compose uint16 from 2 bytes
                                run |= COEF_RECEPTION_FINISH_FLAG; // set the prevent further filter coeffitients reception flag
                        }
                        
                        break;
                
                default: // receive filter coeficients
                        if ( !run && ( rx_msg.id >= FILTER_ROLL_A_ID ) && ( rx_msg.id <= FILTER_MSH_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 and no run-time flag is set, add it to the respective filter coeffitients array
        }
        
        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 uart_err; // UART error code
        
        static uint8_t check_imu; // IMU datagram cross-parity computation variable
        static uint8_t i; // 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
        
        float roll; // Computed roll rate [64 Hz]
        float pitch; // Computed pitch rate [64 Hz]
        float yaw; // Computed yaw rate [64 Hz]
        
        float accel_x; // Computed x-axis acceleration [64 Hz]
        float accel_y; // Computed y-axis acceleration [64 Hz]
        float accel_z; // Computed z-axis acceleration [64 Hz]
        
        float roll_compensated = 0.0; // Computed roll rate [64 Hz] with offset compensation
        float pitch_compensated = 0.0; // Computed pitch rate [64 Hz] with offset compensation
        float yaw_compensated = 0.0; // Computed yaw rate [64 Hz] with offset compensation
        
        static float roll_offset = 0.0; // roll gyro offset
        static float pitch_offset = 0.0; // pitch gyro offset
        static float yaw_offset = 0.0; // yaw gyro offset
        
        static float x_roll[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, y_roll[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // roll rate filtering buffer
        static float x_pitch[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, y_pitch[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // pitch rate filtering buffer
        static float x_yaw[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, y_yaw[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // yaw rate filtering buffer
        static float x_accel_x[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, y_accel_x[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // x-axis acceleration filtering buffer
        static float x_accel_y[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, y_accel_y[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // y-axis acceleration filtering buffer
        static float x_accel_z[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, y_accel_z[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // z-axis acceleration filtering buffer
        static float x_msh_x[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, y_msh_x[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // x-axis magnetometer filtering buffer
        static float x_msh_y[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, y_msh_y[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // y-axis magnetometer filtering buffer
        static float x_msh_z[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, y_msh_z[MAX_FILTER_LEN] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // z-axis magnetometer filtering buffer
        
        can_msg_t msg_rq = {.dlc = 0, .id = RQ_CAN_ID}; // time synchronization CAN message
        
        /* check UART errors */
        if ( ( uart_err = UART_test_err ( UART0 ) ) ) {
                if ( uart_err & ROUND_BUFFER_OVERRUN_ERR_MASK ) { // if round buffer overrun
                        imu_msg_err.data[0] = UART_ROUND_BUFFER_OVERRUN_ERR; // fill error code into the IMU error message structure
                        while ( can_tx_msg ( &imu_msg_err ) ); // send the error message
                }
                
                if ( uart_err & RX_BUFFER_OVERRUN_ERR_MASK ) { // if hardware Rx buffer overrun
                        imu_msg_err.data[0] = UART_RX_BUFFER_OVERRUN_ERR; // fill error code into the IMU error message structure
                        while ( can_tx_msg ( &imu_msg_err ) ); // send the error message
                }
                
                if ( uart_err & PARITY_ERR_MASK ) { // if parity error
                        imu_msg_err.data[0] = UART_PARITY_ERR; // fill error code into the IMU error message structure
                        while ( can_tx_msg ( &imu_msg_err ) ); // send the error message
                }
                
                if ( uart_err & FRAME_ERR_MASK ) { // if framing error
                        imu_msg_err.data[0] = UART_FRAME_ERR; // fill error code into the IMU error message structure
                        while ( can_tx_msg ( &imu_msg_err ) ); // send the error message
                }
                
                if ( uart_err & BREAK_IRQ_ERR_MASK ) { // if interrupt break
                        imu_msg_err.data[0] = UART_BREAK_IRQ_ERR; // fill error code into the IMU error message structure
                        while ( can_tx_msg ( &imu_msg_err ) ); // send the error message
                }
        }
        
        /* state automaton for IMU data reception and processing */
        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 > SAMPLING_FREQ_PRESCALER ) while ( can_tx_msg ( &msg_rq ) ); // when the ( SAMPLING_FREQ_PRESCALER + 1 )-th teleram is being received, send the synchonisation message (IMU running at 64Hz, overall system sampling frequency is 64 / ( SAMPLING_FREQ_PRESCALER + 1 )Hz)
                        } else ( *step ) = 0; // should another character be received, return to the sequence beginning
                        
                        break;
                
                case 2: i = read_UART_data ( UART0 ); // read the next telegram character
                        check_imu = IMU_MES_DELIMITER_1 ^ IMU_MES_DELIMITER_2 ^ i; // cross parity initialization
                        
                        if ( i & IMU_RANGE_MASK ) { // when IMU range error is detected, fill in respective error code
                                imu_msg_err.data[0] = RANGE_ERR; // into the IMU error message structure
                                while ( can_tx_msg ( &imu_msg_err ) ); // send the error message
                        }
                        
                        i = 0; // reset i, as it will be used as a cycle counter in the next two steps
                        ( *step )++;
                        
                        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
                        
                        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[i], buffer_int16, sizeof ( int16_t ) ); // read the second part of a 16-bit integer
                        
                        if ( ++i < 6 ) ( *step )--; else ( *step )++; // when all 6 values are acquired go ahead, or return to the previous step
                        break;
                
                case 5: check_imu ^= read_UART_data ( UART0 ); // read the cross parity - 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[0] = PARITY_ERR; // into the IMU error message structure
                                while ( can_tx_msg ( &imu_msg_err ) ); // send the error message
                        }
                        
                        /* angular rates computation */
                        roll = ( (float) buffer_imu[0] ) * -ANGULAR_CONV; // convert yaw rate to rads/s (*(-1) due to the IMU orientation)
                        pitch = ( (float) buffer_imu[1] ) * ANGULAR_CONV; // convert yaw rate to rads/s
                        yaw = ( (float) buffer_imu[2] ) * -ANGULAR_CONV; // convert yaw rate to rads/s (*(-1) due to the IMU orientation)
                        
                        /* x, y and z acceleration computation */
                        accel_x = ( (float) buffer_imu[4] ) * -ACCEL_CONV; // convert x-axis acceleration into miligs (*(-1) due to the IMU orientation)
                        accel_y = ( (float) buffer_imu[3] ) * ACCEL_CONV; // convert y-axis acceleration into miligs
                        accel_z = ( (float) buffer_imu[5] ) * ACCEL_CONV; // convert z-axis acceleration into miligs
                        
                        /* data filtering - roll, pitch and yaw angular rates, x, y and z accelerations and magnetic intensities */
                        filter ( roll, x_roll, y_roll, a_roll, b_roll, filter_roll_a_num, filter_roll_b_num );
                        filter ( pitch, x_pitch, y_pitch, a_pitch, b_pitch, filter_pitch_a_num, filter_pitch_b_num );
                        filter ( yaw, x_yaw, y_yaw, a_yaw, b_yaw, filter_yaw_a_num, filter_yaw_b_num );
                        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 );
                        filter ( msh_val[2], x_msh_x, y_msh_x, a_msh_x, b_msh_x, filter_msh_x_a_num, filter_msh_x_b_num );
                        filter ( msh_val[0], x_msh_y, y_msh_y, a_msh_y, b_msh_y, filter_msh_y_a_num, filter_msh_y_b_num );
                        filter ( msh_val[1], x_msh_z, y_msh_z, a_msh_z, b_msh_z, filter_msh_z_a_num, filter_msh_z_b_num );
                        
                        /* gyros offset computation (calibration) */
                        if ( !( run & RUN_TIME_FLAG ) && ( 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 ( !( run & RUN_TIME_FLAG ) && ( cal_sample_count == cal_sample_num ) ) { // compute offset mean values
                                roll_offset /= ( (float) ( cal_sample_num - GYRO_OFFSET_FILTERING_TRESHOLD ) );
                                pitch_offset /= ( (float) ( cal_sample_num - GYRO_OFFSET_FILTERING_TRESHOLD ) );
                                yaw_offset /= ( (float) ( 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
                                
                                run |= RUN_TIME_FLAG; // set the run-time flag to prevent calibration
                        }
                        
                        /* filtered roll, pitch and yaw offset compensation */
                        if ( ( run & RUN_TIME_FLAG ) ) {
                                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
                        
                        if ( sample_count > SAMPLING_FREQ_PRESCALER ) { // if the ( SAMPLING_FREQ_PRESCALER + 1 )-th telegram is received, process the data
                                send_float_can ( AR_CAN_ID_ROLL, &roll, &roll_compensated ); // send the CAN message containing roll angular rate
                                send_float_can ( AR_CAN_ID_PITCH, &pitch, &pitch_compensated ); // send the CAN message containing pitch angular rate
                                send_float_can ( AR_CAN_ID_YAW, &yaw, &yaw_compensated ); // send the CAN message containing yaw angular rate
                                send_float_can ( AC_CAN_ID_X, &accel_x, &y_accel_x[filter_accel_x_a_num - 1] ); // send the CAN message containing x-axis acceleration
                                send_float_can ( AC_CAN_ID_Y, &accel_y, &y_accel_y[filter_accel_y_a_num - 1] ); // send the CAN message containing y-axis acceleration
                                send_float_can ( AC_CAN_ID_Z, &accel_z, &y_accel_z[filter_accel_z_a_num - 1] ); // send the CAN message containing z-axis acceleration
                                
                                /* MSH messages */
                                send_float_can ( MSH_ID_X, &msh_val[2], &y_msh_x[filter_msh_x_a_num - 1] ); // send the CAN message (x magnetic field intensity)
                                send_float_can ( MSH_ID_Y, msh_val, &y_msh_y[filter_msh_y_a_num - 1] ); // send the CAN message (y magnetic field intensity)
                                send_float_can ( MSH_ID_Z, &msh_val[1], &y_msh_z[filter_msh_z_a_num - 1] ); // send the CAN message (z magnetic field intensity)
                                msh_sent_flag = 1;
                                
                                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[0] = 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
                                        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
        }
}

/*! 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 ) {
        const 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
        uint8_t uart_err; // UART error code
        
        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
        
        /* check UART errors */
        if ( ( uart_err = UART_test_err ( UART1 ) ) ) {
                if ( uart_err & ROUND_BUFFER_OVERRUN_ERR_MASK ) { // if round buffer overrun
                        gps_msg_err.data[0] = UART_ROUND_BUFFER_OVERRUN_ERR; // fill error code into the GPS error message structure
                        while ( can_tx_msg ( &gps_msg_err ) ); // send the error message
                }
                
                if ( uart_err & RX_BUFFER_OVERRUN_ERR_MASK ) { // if hardware Rx buffer overrun
                        gps_msg_err.data[0] = UART_RX_BUFFER_OVERRUN_ERR; // fill error code into the GPS error message structure
                        while ( can_tx_msg ( &gps_msg_err ) ); // send the error message
                }
                
                if ( uart_err & PARITY_ERR_MASK ) { // if parity error
                        gps_msg_err.data[0] = UART_PARITY_ERR; // fill error code into the GPS error message structure
                        while ( can_tx_msg ( &gps_msg_err ) ); // send the error message
                }
                
                if ( uart_err & FRAME_ERR_MASK ) { // if framing error
                        gps_msg_err.data[0] = UART_FRAME_ERR; // fill error code into the GPS error message structure
                        while ( can_tx_msg ( &gps_msg_err ) ); // send the error message
                }
                
                if ( uart_err & BREAK_IRQ_ERR_MASK ) { // if interrupt break
                        gps_msg_err.data[0] = UART_BREAK_IRQ_ERR; // fill error code into the GPS error message structure
                        while ( can_tx_msg ( &gps_msg_err ) ); // send the error message
                }
        }
        
        /* state automaton for GPS data reception and processing */
        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)
                                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[0] = PARITY_ERR; // into the IMU error message structure
                                while ( can_tx_msg ( &gps_msg_err ) ); // send 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[0] = FRAME_ERR; // fill the respective error code into the IMU error message structure
                                while ( can_tx_msg ( &gps_msg_err ) ); // send 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[0] = FRAME_ETX_ERR; // 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
                        }
                        
                        /* compose and send the CAN messages containing the GPS data */
                        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
                        }
                        
                        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_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[0] = 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
                                LED_ERR_IOCLR |= LED_ERR_IOBIT; // switch the ERR LED off
                                gps_err_state = 0; // turn off the GPS error flag
                        }
        }
}

/*! 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
        uint32_t count = 0; // auxiliary variables for wait cycle
        
        can_msg_t reset_msg = {.id = DAM_RESET_ID, .dlc = 2, .data = {MAIN_VERSION, SUB_VERSION}}; // GPS CAN data packet
        
        sys_pll_init ( CCLK, OSC_FREQ ); // set the core clock to 60MHz
        
        VPBDIV = 0x01; // peripheral clock = CPU clock (60MHz)
        
        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 on
        LED_IMU_IOSET |= 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 on
        LED_GPS_IOSET |= 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 on
        LED_MSH_IOSET |= 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, PWM_PERIOD ); // init PWM
        pwm_set ( 2, PWM_PULSE_LENGTH ); // switch on Villard
        
        can_init ( CAN_BTR, ISR_INT_VEC_CAN, NULL ); // init CAN
        watchdog_init ( WATCHDOG_INIT ); // watchdog initialization
        
        while ( can_tx_msg ( &reset_msg ) ); // send the DAM reset message
        
        while ( ( count < VILLARD_CHARGE_WAIT ) || !( run & COEF_RECEPTION_FINISH_FLAG ) ) { // wait for Villard to charge and filter coeffitients reception
                if ( can_msg_received ) can_rx ( ); // when CAN receives new data, call the service routine
                if ( !( run & RESET_REQUEST_FLAG ) && ( WDTV < ( WATCHDOG_INIT / 2 ) ) ) watchdog_reset ( ); // watchdog reset
                if ( ( run & RUN_TIME_FLAG ) ) break; // if run-time flag is set, skip the waiting
                if ( count < VILLARD_CHARGE_WAIT ) count++;
        }
        
        run |= COEF_RECEPTION_FINISH_FLAG; // set the prevent further filter coeffitients reception flag
        
        adc_init ( ADC_PRESCALER ); // init AD converter
        UART_init ( UART0, BAUD_RATE0, PCLK, ISR_INT_VEC_U0 ); // init UART0
        UART_init ( UART1, BAUD_RATE1, PCLK, ISR_INT_VEC_U1 ); // init UART1
        timer_init ( TIMER_PRESCALER, TIMER_PERIOD, ISR_INT_VEC_TIMER ); // init timer0
        
        LED_IMU_IOCLR |= LED_IMU_IOBIT; // switch the IMU LED off
        LED_GPS_IOCLR |= LED_GPS_IOBIT; // switch the GPS LED off
        LED_MSH_IOCLR |= LED_MSH_IOBIT; // switch the MSH LED off
        LED_ERR_IOCLR |= LED_ERR_IOBIT; // switch the ERR LED off
        
        /* 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
        }
}