/* Copyright (c) 2015 Nordic Semiconductor. All Rights Reserved. * * The information contained herein is property of Nordic Semiconductor ASA. * Terms and conditions of usage are described in detail in NORDIC * SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT. * * Licensees are granted free, non-transferable use of the information. NO * WARRANTY of ANY KIND is provided. This heading must NOT be removed from * the file. * */ #include "nrf_drv_saadc.h" #include "nrf.h" #include "nrf_gzll.h" // #include "nrf_gzll_error.h" // #include "LSM_330_Registrai.h" #include "nrf_drv_spi.h" #include "nrf_drv_gpiote.h" #include "app_util_platform.h" #include "nrf_gpio.h" #include "nrf_drv_saadc.h" //#include "nrf_drv_ppi.h" #include "nrf_delay.h" #include "nrf_log.h" #include "boards.h" #include "app_error.h" #include "app_timer.h" #include #include "nrf_drv_clock.h" #include "nrf_drv_rtc.h" #include "math.h" typedef union{ struct{ uint32_t A0 : 1; // Bat data collected uint32_t A1 : 1; // Gyr data colected uint32_t A2 : 1; // Acc data collected uint32_t A3 : 1; uint32_t A5 : 1; uint32_t A6 : 1; uint32_t A7 : 1; uint32_t A8 : 1; } flag; uint32_t all_flags; } s_system_flags; extern volatile s_system_flags sys; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>SPI<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< #define PI 3.14159265358979323846 #define SPI_CS_Acc 7 #define SPI_CS_Gyr 8 #define SDO_Acc 4 #define SDO_Gyr 5 #define DIN 3 #define DRDY_PIN_GYR 15 #define DRDY_PIN_ACC 12 #define SCLK 6 #define SPI_INSTANCE 0 /**< SPI instance index. */ #define RESTRICT_PITCH static const nrf_drv_spi_t spi = NRF_DRV_SPI_INSTANCE(SPI_INSTANCE); /**< SPI instance. */ static volatile bool spi_xfer_done; /**< Flag used to indicate that SPI instance completed the transfer. */ bool Gyr_status=0; int16_t data_acc[3]; int16_t data_gyr[3]; float acc[3]; float DPS[3]; float g=9.81; uint8_t m_tx_buf[10]; /**< TX buffer. */ uint8_t m_rx_buf[10]; /**< RX buffer. */ bool lsm_330_wr_config_acc_done=0; bool lsm_330_wr_config_gyr_done=0; bool angle_calc_done=false; float roll; float pitch; float accX; float accY; float accZ; float gyroYrate; float gyroXrate; float RAD_TO_DEG=180/PI; float gyroXangle; float gyroYangle; //double compAngleX; //double compAngleY; float dt=1; // Priklauso nuo zemesnio sensoriaus daznio bool first_loop=false; float kalAngleX; float kalAngleY; bool acc_on; bool gyr_on; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>SAADC<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< #define SAMPLES_IN_BUFFER 1 #define SAADC_BURST_MODE 1 #define RTC_CC_VALUE 32678 //Determines the RTC interrupt frequency and thereby the SAADC sampling frequency #define SAADC_CALIBRATION_INTERVAL 10 //Determines how often the SAADC should be calibrated relative to NRF_DRV_SAADC_EVT_DONE event. E.g. value 5 will make the SAADC calibrate every fifth time the NRF_DRV_SAADC_EVT_DONE is received. #define SAADC_SAMPLES_IN_BUFFER 1 //Number of SAADC samples in RAM before returning a SAADC event. For low power SAADC set this constant to 1. Otherwise the EasyDMA will be enabled for an extended time which consumes high current. bool m_saadc_initialized=0; static nrf_saadc_value_t m_buffer_pool[1][SAMPLES_IN_BUFFER]; static uint32_t m_adc_evt_counter; float C_BAT; double vbat; const nrf_drv_rtc_t rtc; const nrf_drv_rtc_t rtc = NRF_DRV_RTC_INSTANCE(0); /**< Declaring an instance of nrf_drv_rtc for RTC2. */ //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>GAZELL<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< // Define pipe #define PIPE_NUMBER 2 ///< use pipes 1 and 2 char nr='2'; //akselerometro nr // Define payload length #define TX_PAYLOAD_LENGTH 32 ///< We use 1 byte payload length when transmitting #define MAX_TX_ATTEMPTS 5 /**< Maximum number of transmission attempts */ #define APP_TIMER_PRESCALER 0 /**< Value of the RTC PRESCALER register. */ #define APP_TIMER_OP_QUEUE_SIZE 8u /**< Size of timer operation queues. */ #define NRF_GZLL_CHANNEL_TAB {17, 3, 27} //{17, 3, 27} // {11, 19, 9} for 1 < Redefine channel table for example. #define NRF_GZLL_CHANNEL_TAB_SIZE 3 // Data and acknowledgement payloads static uint8_t data_payload[TX_PAYLOAD_LENGTH]; ///< Payload to send to Host. static uint8_t ack_payload[NRF_GZLL_CONST_MAX_PAYLOAD_LENGTH]; ///< Placeholder for received ACK payloads from Host. // Debug helper variables static volatile bool init_ok, enable_ok, push_ok, pop_ok, tx_success; uint8_t input_get[32]; bool rx_payload_present; bool acc_data_collected; bool gyr_data_collected; bool bat_data_collected; uint32_t timeslot_period=600; nrf_gzll_datarate_t datarate=NRF_GZLL_DATARATE_2MBIT; uint32_t timeslots_per_channel=8; uint32_t packet_nr=0; uint32_t tx_sucess; /*****************************************************************************/ /** @name SPI callback function definitions */ /*****************************************************************************/ void lsm_330_wr_register(uint8_t SPI_CS, uint8_t registras, uint8_t duomenys) // rasome duomenis i registra { nrf_gpio_pin_clear(SPI_CS);// CS to LOW, ON memset(&m_rx_buf, 0, 7); memset(&m_tx_buf, 0, 3); m_tx_buf[0]=registras; m_tx_buf[1]=duomenys; nrf_drv_spi_transfer(&spi, &m_tx_buf[0], 2, &m_rx_buf[0], 0); // start from m_tx_buf[0[, send 2 bytes, start receving at m_rx_buf[[1], receive 0 bytes while(spi_xfer_done == false) // wait for SENT interrupt { __WFE(); } spi_xfer_done=false;// Clear SENT flag nrf_gpio_pin_set(SPI_CS);// CS to HiGH, OFF } void lsm_330_read_register(uint8_t SPI_CS, uint8_t registras) // skaitome duomenis is registro { nrf_gpio_pin_clear(SPI_CS);// CS GYR to LOW, ON memset(&m_rx_buf, 0, 7); memset(&m_tx_buf, 0, 3); m_tx_buf[0]=SPI_READ|registras; nrf_drv_spi_transfer(&spi, &m_tx_buf[0], 2, &m_rx_buf[0], 2); while(spi_xfer_done == false) // wait for SENT interrupt { __WFE(); } spi_xfer_done=false; // Clear SENT flag nrf_gpio_pin_set(SPI_CS);// CS GYR to HiGH, OFF } void lsm_330_read_data_Acc() // skaitome duomenis is registro { spi_xfer_done=false;// Clear SENT flag nrf_gpio_pin_clear(SPI_CS_Acc);// CS ACC to LOW, ON acc_on=true; memset(&m_rx_buf, 0, 10); memset(&m_tx_buf, 0, 10); m_tx_buf[0]=OUT_X_L_A|LSM_AUTO_INCREMENT_READ; nrf_drv_spi_transfer(&spi, &m_tx_buf[0], 7, &m_rx_buf[0],7); } void spi_event_handler(nrf_drv_spi_evt_t const * p_event) { if(acc_on==!0) { packet_nr++; for(int i=0; i<3;i++) { data_acc[i]=((m_rx_buf[(i+1)*2]<<8)+m_rx_buf[i*2+1]); data_acc[i]=data_acc[i]/16; acc[i]=data_acc[i]; acc[i]=acc[i]*2*g; acc[i]=acc[i]/2047; } accX=acc[0]; accY=acc[1]; accZ=acc[2]; if (first_loop==false) { #ifdef RESTRICT_PITCH // Eq. 25 and 26 roll = (atan2(accY, accZ)* RAD_TO_DEG); pitch = (atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG); #else // Eq. 28 and 29 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; double pitch = atan2(-accX, accZ) * RAD_TO_DEG; #endif kalAngleX=roll; kalAngleY=pitch; first_loop=true; } for(int i=0; i<6;i++) { input_get[i]=m_rx_buf[i+1]; } acc_data_collected=1; nrf_gpio_pin_set(SPI_CS_Acc);// CS ACC to HiGH, OFF acc_on=false; } if (gyr_on==!0) { for(int i=0; i<3;i++) for(int i=0; i<3;i++) { data_gyr[i]=((m_rx_buf[(i+1)*2]<<8)+m_rx_buf[i*2+1]); data_gyr[i]=data_gyr[i]/16; DPS[i]=data_gyr[i]; DPS[i]=DPS[i]/2047; DPS[i]=DPS[i]*200; } gyroXrate=DPS[0]; gyroYrate=DPS[1]; for(int i=0; i<6;i++) { input_get[6+i]=m_rx_buf[i+1]; } gyr_data_collected=1; nrf_gpio_pin_set(SPI_CS_Gyr);// CS GYR to HiGH, OFF gyr_on=false; } spi_xfer_done = true; } void lsm_330_read_data_Gyr() // skaitome duomenis is registro { spi_xfer_done=false;// Clear SENT flag nrf_gpio_pin_clear(SPI_CS_Gyr);// CS GYR to LOW, ON gyr_on=true; memset(&m_rx_buf, 0, 10); memset(&m_tx_buf, 0, 10); m_tx_buf[0]=OUT_X_L_G|LSM_AUTO_INCREMENT_READ; nrf_drv_spi_transfer(&spi, &m_tx_buf[0], 7, &m_rx_buf[0],7); // for(int i=0; !((i>100)|spi_xfer_done); i++ ) // wait for SENT interrupt // { // //nrf_delay_ms(100); // __WFE(); // } } void SPI_ON() // Pajungiame SPI akselerometrui { //SPI default nustatymai // #define NRF_DRV_SPI_DEFAULT_CONFIG(id) \ //{ \ // .sck_pin = CONCAT_3(SPI, id, _CONFIG_SCK_PIN), \ // .mosi_pin = CONCAT_3(SPI, id, _CONFIG_MOSI_PIN), \ // .miso_pin = CONCAT_3(SPI, id, _CONFIG_MISO_PIN), \ // .ss_pin = NRF_DRV_SPI_PIN_NOT_USED, \ // .irq_priority = CONCAT_3(SPI, id, _CONFIG_IRQ_PRIORITY), \ // .orc = 0xFF, \ // .frequency = NRF_DRV_SPI_FREQ_4M, \ // .mode = NRF_DRV_SPI_MODE_0, \ // .bit_order = NRF_DRV_SPI_BIT_ORDER_MSB_FIRST, nrf_drv_spi_config_t spi_config = NRF_DRV_SPI_DEFAULT_CONFIG(SPI_INSTANCE); spi_config.sck_pin = SCLK; spi_config.mosi_pin = DIN; spi_config.miso_pin = SDO_Acc; spi_config.ss_pin = NRF_DRV_SPI_PIN_NOT_USED; APP_ERROR_CHECK(nrf_drv_spi_init(&spi, &spi_config, spi_event_handler)); } void config_Acc() { lsm_330_wr_register(SPI_CS_Acc, CTRL_REG1_A, LSM_ACC_DATARATE_100Hz|LSM_ACC_ALL_EN);//|LSM_ACC_LOW_POWER_EN); nrf_delay_ms(700); // pagal datasheet reikia palaukti po ijungimo lsm_330_wr_register(SPI_CS_Acc, CTRL_REG4_A,LSM_ACC_FULL_SCALE_2G|LSM_ACC_HIGH_RES); lsm_330_wr_register(SPI_CS_Gyr, CTRL_REG5_A,LSM_ACC_FIFO_EN); lsm_330_wr_register(SPI_CS_Gyr,LSM_ACC_FIFO_CTRL_ADDRESS,LSM_ACC_FIFO_BYPASS_MODE); lsm_330_wr_register(SPI_CS_Acc, CTRL_REG3_A,LSM_ACC_DRDY1_INT); lsm_330_wr_config_acc_done=1; } void config_Gyr() { lsm_330_wr_register(SPI_CS_Gyr, CTRL_REG1_G,LSM_GYRO_DATARATE_95HZ|LSM_GYRO_ALL_EN|LSM_GYRO_POWER_ON); nrf_delay_ms(100); //laukiama po ijungimo lsm_330_wr_register(SPI_CS_Gyr, CTRL_REG5_G,LSM_GYRO_FIFO_EN); lsm_330_wr_register(SPI_CS_Gyr,LSM_GYRO_FIFO_CTRL_ADDRESS,LSM_GYRO_FIFO_BYPASS_MODE); lsm_330_wr_register(SPI_CS_Gyr, CTRL_REG3_G,LSM_GYRO_DRDY_INT); lsm_330_wr_config_gyr_done=1; } void DRDY_Acc(nrf_drv_gpiote_pin_t pin, nrf_gpiote_polarity_t action) //ACC DRDY interruptas { if (lsm_330_wr_config_acc_done && !acc_data_collected && spi_xfer_done) { lsm_330_read_data_Acc(); } } void DRDY_Gyr(nrf_drv_gpiote_pin_t pin, nrf_gpiote_polarity_t action) //Gyr DRDY interruptas { if (lsm_330_wr_config_gyr_done && !gyr_data_collected && spi_xfer_done) { lsm_330_read_data_Gyr(); } } static void DRDY_Acc_init(void) // Gyr DRDY interrupto paleidimas { ret_code_t err_code; nrf_drv_gpiote_in_config_t in_config_1 = GPIOTE_CONFIG_IN_SENSE_LOTOHI(true); in_config_1.pull = NRF_GPIO_PIN_PULLUP; err_code = nrf_drv_gpiote_in_init(DRDY_PIN_ACC, &in_config_1, DRDY_Acc); APP_ERROR_CHECK(err_code); nrf_drv_gpiote_in_event_enable(DRDY_PIN_ACC, true); NVIC_SetPriority(GPIOTE_IRQn, 6); } static void DRDY_Gyr_init(void) // Gyr DRDY interrupto paleidimas { ret_code_t err_code; err_code = nrf_drv_gpiote_init(); APP_ERROR_CHECK(err_code); nrf_drv_gpiote_in_config_t in_config = GPIOTE_CONFIG_IN_SENSE_LOTOHI(true); in_config.pull = NRF_GPIO_PIN_PULLUP; err_code = nrf_drv_gpiote_in_init(DRDY_PIN_GYR, &in_config, DRDY_Gyr); APP_ERROR_CHECK(err_code); nrf_drv_gpiote_in_event_enable(DRDY_PIN_GYR, true); NVIC_SetPriority(GPIOTE_IRQn, 6); } /*****************************************************************************/ /** @name SAADC callback function definitions */ /*****************************************************************************/ void saadc_callback(nrf_drv_saadc_evt_t const * p_event) { if (p_event->type == NRF_DRV_SAADC_EVT_DONE) { ret_code_t err_code; if((m_adc_evt_counter % SAADC_CALIBRATION_INTERVAL) == 0) //Evaluate if offset calibration should be performed. Configure the SAADC_CALIBRATION_INTERVAL constant to change the calibration frequency { NRF_SAADC->EVENTS_CALIBRATEDONE = 0; //Clear the calibration event flag nrf_saadc_task_trigger(NRF_SAADC_TASK_CALIBRATEOFFSET); //Trigger calibration task while(!NRF_SAADC->EVENTS_CALIBRATEDONE); //Wait until calibration task is completed. The calibration tasks takes about 1000us with 10us acquisition time. Configuring shorter or longer acquisition time will make the calibration take shorter or longer respectively. while(NRF_SAADC->STATUS == (SAADC_STATUS_STATUS_Busy << SAADC_STATUS_STATUS_Pos)); //Additional wait for busy flag to clear. Without this wait, calibration is actually not completed. This may take additional 100us - 300us } err_code = nrf_drv_saadc_buffer_convert(p_event->data.done.p_buffer, SAMPLES_IN_BUFFER); //Set buffer so the SAADC can write to it again. This is either "buffer 1" or "buffer 2" input_get[13]=(p_event->data.done.p_buffer[0])>>8; // High bit input_get[12]=(p_event->data.done.p_buffer[0])&0xFF; // Low bit // vbat=(p_event->data.done.p_buffer[0]*3)/4096.0; // vbat=(vbat*(500000+500000))/500000; // if(vbat>=3.7) // C_BAT = (unsigned char)((float)(-218*vbat*vbat)+(1901*vbat)-4039); // else if (vbat>=3.3) // C_BAT = (unsigned char)((float)(38.6*vbat*vbat)-(249*vbat)+402); // else // C_BAT = 0; // if (C_BAT > 100){ C_BAT = 100;}; // //4096; APP_ERROR_CHECK(err_code); err_code = nrf_drv_saadc_buffer_convert(p_event->data.done.p_buffer, SAMPLES_IN_BUFFER); APP_ERROR_CHECK(err_code); m_adc_evt_counter++; //SAADC ISSIJUNGIA nrf_drv_saadc_uninit(); //Unintialize SAADC to disable EasyDMA and save power NRF_SAADC->INTENCLR = (SAADC_INTENCLR_END_Clear << SAADC_INTENCLR_END_Pos); //Disable the SAADC interrupt NVIC_ClearPendingIRQ(SAADC_IRQn); //Clear the SAADC interrupt if set } } void saadc_init(void) { // SAADC channel configurations #define Nustatymai \ { \ .resistor_p = NRF_SAADC_RESISTOR_DISABLED, \ .resistor_n = NRF_SAADC_RESISTOR_DISABLED, \ .gain = NRF_SAADC_GAIN1_5, \ .reference = NRF_SAADC_REFERENCE_INTERNAL, \ .acq_time = NRF_SAADC_ACQTIME_40US, \ .mode = NRF_SAADC_MODE_SINGLE_ENDED, \ .pin_p = NRF_SAADC_INPUT_AIN4, \ .pin_n = NRF_SAADC_INPUT_DISABLED \ } //SAADC configurations #define nustat \ { \ .resolution=NRF_SAADC_RESOLUTION_12BIT, \ .oversample=NRF_SAADC_OVERSAMPLE_8X, \ .interrupt_priority = SAADC_CONFIG_IRQ_PRIORITY \ } ret_code_t err_code; nrf_drv_saadc_config_t configas=nustat; // uzkrauna SAADC nustatymus nrf_saadc_channel_config_t channel_config =Nustatymai; // Uzkrauna kanalo nustatymus //Initialize SAADC err_code = nrf_drv_saadc_init(&configas, saadc_callback); APP_ERROR_CHECK(err_code); //Initialize SAADC channel err_code = nrf_drv_saadc_channel_init(4, &channel_config); //Initialize SAADC channel 4 with the channel configuration APP_ERROR_CHECK(err_code); //Enable Burst mode if(SAADC_BURST_MODE) { NRF_SAADC->CH[4].CONFIG |= 0x01000000; //Configure burst mode for channel 0. Burst is useful together with oversampling. When triggering the SAMPLE task in burst mode, the SAADC will sample "Oversample" number of times as fast as it can and then output a single averaged value to the RAM buffer. If burst mode is not enabled, the SAMPLE task needs to be triggered "Oversample" number of times to output a single averaged value to the RAM buffer. } //Set SAADC buffer 1. The SAADC will start to write to this buffer err_code = nrf_drv_saadc_buffer_convert(m_buffer_pool[0],SAMPLES_IN_BUFFER); APP_ERROR_CHECK(err_code); } static void rtc_handler(nrf_drv_rtc_int_type_t int_type) { uint32_t err_code; if(!bat_data_collected) { saadc_init(); nrf_drv_saadc_sample(); //Trigger the SAADC SAMPLE task nrf_delay_ms(1); } err_code = nrf_drv_rtc_cc_set(&rtc,0,RTC_CC_VALUE,true); //Set RTC compare value. This needs to be done every time as the nrf_drv_rtc clears the compare register on every compare match APP_ERROR_CHECK(err_code); nrf_drv_rtc_counter_clear(&rtc); //Clear the RTC counter to start count from zero bat_data_collected=1; } static void lfclk_config(void) { ret_code_t err_code = nrf_drv_clock_init(); //Initialize the clock source specified in the nrf_drv_config.h file, i.e. the CLOCK_CONFIG_LF_SRC constant APP_ERROR_CHECK(err_code); nrf_drv_clock_lfclk_request(NULL); } static void rtc_config(void) { uint32_t err_code; //Initialize RTC instance err_code = nrf_drv_rtc_init(&rtc, NULL, rtc_handler); //Initialize the RTC with callback function rtc_handler. The rtc_handler must be implemented in this applicaiton. Passing NULL here for RTC configuration means that configuration will be taken from the nrf_drv_config.h file. APP_ERROR_CHECK(err_code); err_code = nrf_drv_rtc_cc_set(&rtc,0,RTC_CC_VALUE,true); //Set RTC compare value to trigger interrupt. Configure the interrupt frequency by adjust RTC_CC_VALUE and RTC0_CONFIG_FREQUENCY constant in the nrf_drv_config.h file APP_ERROR_CHECK(err_code); //Power on RTC instance nrf_drv_rtc_enable(&rtc); //Enable RTC } /*****************************************************************************/ /** @name Gazell callback function definitions */ /*****************************************************************************/ /** * @brief TX success callback. * * @details If an ACK was received, another packet is sent. */ void nrf_gzll_device_tx_success(uint32_t pipe, nrf_gzll_device_tx_info_t tx_info) { bool result_value = false; uint32_t ack_payload_length = NRF_GZLL_CONST_MAX_PAYLOAD_LENGTH; tx_success=1; if (tx_info.payload_received_in_ack) { result_value = nrf_gzll_fetch_packet_from_rx_fifo(pipe, ack_payload, &ack_payload_length); } // Load data payload into the TX queue. // result_value = nrf_gzll_add_packet_to_tx_fifo(pipe, data_payload, TX_PAYLOAD_LENGTH); } /** * @brief TX failed callback. * * @details If the transmission failed, send a new packet. * * @warning This callback does not occur by default since NRF_GZLL_DEFAULT_MAX_TX_ATTEMPTS * is 0 (inifinite retransmits). */ void nrf_gzll_device_tx_failed(uint32_t pipe, nrf_gzll_device_tx_info_t tx_info) { tx_success=0; // Load data into TX queue. memcpy(&data_payload,&input_get,32); //data_payload[0] = input_get[0]; bool result_value = nrf_gzll_add_packet_to_tx_fifo(pipe, data_payload, 28); } /** * @brief Gazelle callback. * @warning Required for successful Gazell initialization. */ void nrf_gzll_host_rx_data_ready(uint32_t pipe, nrf_gzll_host_rx_info_t rx_info) { } /** * @brief Gazelle callback. * @warning Required for successful Gazell initialization. */ void nrf_gzll_disabled() { } /*****************************************************************************/ /** @name KALMAN FILTER*/ /*****************************************************************************/ // KasBot V1 - Kalman filter module float Q_angle = 0.001; //0.001 float Q_gyro = 0.003; //0.003 float R_angle = 0.03; //0.03 float x_bias = 0; float XP_00 = 0, XP_01 = 0, XP_10 = 0, XP_11 = 0; float Xy, XS; float XK_0, XK_1; // newAngle = angle measured with atan2 using the accelerometer // newRate = angle measured using the gyro // looptime = loop time in millis() float kalmanX(float newAngle, float newRate) { //float dt = (float)looptime/1000; kalAngleX += dt * (newRate - x_bias); XP_00 += - dt * (XP_10 + XP_01) + Q_angle * dt; XP_01 += - dt * XP_11; XP_10 += - dt * XP_11; XP_11 += + Q_gyro * dt; Xy = newAngle - kalAngleX; XS = XP_00 + R_angle; XK_0 = XP_00 / XS; XK_1 = XP_10 / XS; kalAngleX += XK_0 * Xy; x_bias += XK_1 * Xy; XP_00 -= XK_0 * XP_00; XP_01 -= XK_0 * XP_01; XP_10 -= XK_1 * XP_00; XP_11 -= XK_1 * XP_01; return kalAngleX; } float y_bias = 0; float YP_00 = 0, YP_01 = 0, YP_10 = 0, YP_11 = 0; float Yy, YS; float YK_0, YK_1; // newAngle = angle measured with atan2 using the accelerometer // newRate = angle measured using the gyro // looptime = loop time in millis() double kalmanY(float newAngle, float newRate) { //float dt = (float)looptime/1000; kalAngleY += dt * (newRate - y_bias); YP_00 += - dt * (YP_10 + YP_01) + Q_angle * dt; YP_01 += - dt * YP_11; YP_10 += - dt * YP_11; YP_11 += + Q_gyro * dt; Yy = newAngle - kalAngleY; YS = YP_00 + R_angle; YK_0 = YP_00 / YS; YK_1 = YP_10 / YS; kalAngleY += YK_0 * Yy; y_bias += YK_1 * Yy; YP_00 -= YK_0 * YP_00; YP_01 -= YK_0 * YP_01; YP_10 -= YK_1 * YP_00; YP_11 -= YK_1 * YP_01; return kalAngleY; } //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>MAIN<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< int main(void) { tx_success=true; dt=(dt/190); // Initialize application timer. APP_TIMER_INIT(APP_TIMER_PRESCALER, APP_TIMER_OP_QUEUE_SIZE, NULL); nrf_gpio_cfg_input(DRDY_PIN_GYR, NRF_GPIO_PIN_NOPULL); nrf_gpio_cfg_input(DRDY_PIN_ACC, NRF_GPIO_PIN_NOPULL); nrf_gpio_cfg_output(SPI_CS_Acc); nrf_gpio_cfg_output(SPI_CS_Gyr); nrf_gpio_pin_set(SPI_CS_Acc); nrf_gpio_pin_set(SPI_CS_Gyr); SPI_ON(); nrf_delay_ms(1); config_Acc(); config_Gyr(); // Initialize Gazell. bool result_value = nrf_gzll_init(NRF_GZLL_MODE_DEVICE); // GAZELLE_ERROR_CODE_CHECK(result_value); result_value = nrf_gzll_set_timeslot_period(timeslot_period); result_value = nrf_gzll_set_datarate(datarate); result_value=nrf_gzll_set_timeslots_per_channel(timeslots_per_channel); uint8_t table[] = NRF_GZLL_CHANNEL_TAB; result_value = nrf_gzll_set_channel_table(table, NRF_GZLL_CHANNEL_TAB_SIZE); // Attempt sending every packet up to MAX_TX_ATTEMPTS times. result_value = nrf_gzll_set_max_tx_attempts(MAX_TX_ATTEMPTS); // GAZELLE_ERROR_CODE_CHECK(result_value); lfclk_config(); //Configure low frequency 32kHz clock rtc_config(); //Configure RTC. The RTC will generate periodic interrupts. Requires 32kHz clock to operate. DRDY_Gyr_init(); // Initialise gyroscope interrupt DRDY_Acc_init(); // Initialise accelerometer interrupt while(Gyr_status==0) { lsm_330_read_register(SPI_CS_Gyr, WHO_AM_I_G); //Pasitikrinam ar Gyr gyvas if(m_rx_buf[1]==0xD4) { Gyr_status=1; // if Gyr_status=0 - mesk errora } } if(nrf_gpio_pin_read(DRDY_PIN_GYR) && spi_xfer_done) {lsm_330_read_data_Gyr(); } if(nrf_gpio_pin_read(DRDY_PIN_ACC)&& spi_xfer_done) {lsm_330_read_data_Acc(); kalAngleX=roll; kalAngleY=pitch; } // Enable Gazell to start sending over the air. result_value = nrf_gzll_enable(); // GAZELLE_ERROR_CODE_CHECK(result_value); spi_xfer_done=1; while(1) { if(nrf_gpio_pin_read(DRDY_PIN_GYR)==!0 && !gyr_data_collected && spi_xfer_done) { lsm_330_read_data_Gyr(); } if(nrf_gpio_pin_read(DRDY_PIN_ACC)==!0 && !acc_data_collected && spi_xfer_done) { lsm_330_read_data_Acc(); } if(gyr_data_collected && acc_data_collected && !angle_calc_done) // angle calculations { #ifdef RESTRICT_PITCH // Eq. 25 and 26 roll = atan2(accY, accZ) * RAD_TO_DEG; pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; #else // Eq. 28 and 29 roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG; pitch = atan2(-accX, accZ) * RAD_TO_DEG; #endif #ifdef RESTRICT_PITCH // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { // kalmanX.setAngle(roll); //naudojama paskaiciuoti Kalmono filtre??? // compAngleX = roll; kalAngleX = roll; gyroXangle = roll; } else kalAngleX = kalmanX(roll, gyroXrate); // Calculate the angle using a Kalman filter if (fabs(kalAngleX) > 90) gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading kalAngleY = kalmanY(pitch, gyroYrate); #else // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { // kalmanY.setAngle(pitch); compAngleY = pitch; kalAngleY = pitch; gyroYangle = pitch; } else kalAngleY = kalmanY(pitch, gyroYrate); // Calculate the angle using a Kalman filter if (fabs(kalAngleY) > 90) gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading kalAngleX = kalmanX(roll, gyroXrate); // Calculate the angle using a Kalman filter #endif gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter gyroYangle += gyroYrate * dt; //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate //gyroYangle += kalmanY.getRate() * dt; //compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter //compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; // Reset the gyro angle when it has drifted too much if (gyroXangle < -180 || gyroXangle > 180) gyroXangle = kalAngleX; if (gyroYangle < -180 || gyroYangle > 180) gyroYangle = kalAngleY; union ufloat { float f; unsigned u; }; union ufloat u1; u1.f = kalAngleX; input_get[14]=u1.u>>24; input_get[15]=(u1.u>>16)&0xFF; input_get[16]=(u1.u>>8)&0xFF; input_get[17]=u1.u&0xFF; u1.f=kalAngleY; input_get[18]=u1.u>>24; input_get[19]=(u1.u>>16)&0xFF; input_get[20]=(u1.u>>8)&0xFF; input_get[21]=u1.u&0xFF; input_get[22]='A'; input_get[23]=nr; angle_calc_done=true; } if(gyr_data_collected && acc_data_collected && tx_success && angle_calc_done) { input_get[24]=packet_nr>>24; input_get[25]=(packet_nr>>16)&0xFF; input_get[26]=(packet_nr>>8)&0xFF; input_get[27]=packet_nr&0xFF; memcpy(&data_payload,&input_get,32); gyr_data_collected=0; acc_data_collected=0; bat_data_collected=0; tx_success=0; angle_calc_done=0; result_value = nrf_gzll_add_packet_to_tx_fifo(PIPE_NUMBER, data_payload, 28); } __WFI(); } }