void parse_ins_msg( void ) { while (InsLink(ChAvailable())) { uint8_t ch = InsLink(Getch()); if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { if(CHIMU_DATA.m_MsgID==0x03) { new_ins_attitude = 1; RunOnceEvery(25, LED_TOGGLE(3) ); if (CHIMU_DATA.m_attitude.euler.phi > M_PI) { CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; } EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta); EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta); } else if(CHIMU_DATA.m_MsgID==0x02) { RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2])); } } } }
void xsens_event_task( void ) { while (Xsens1Link(ChAvailable()) && !xsens_msg_received[0]) { parse_xsens_msg(0, Xsens1Link(Getch())); } /* while (Xsens2Link(ChAvailable()) && !xsens_msg_received[1]) parse_xsens_msg(1, Xsens2Link(Getch())); */ for (int i = 0; i < XSENS_COUNT; i++) { if (xsens_msg_received[i]) { // first make room while (xsens_msg_buf_count[i] >= XSENS_MSG_BUF) { // throwing away old stuff xsens_msg_buf_ci[i] = (xsens_msg_buf_ci[i] + 1 ) % XSENS_MSG_BUF; xsens_msg_buf_count[i]--; } xsens_msg_buf_pi[i] = (xsens_msg_buf_pi[i] + 1 ) % XSENS_MSG_BUF; xsens_msg_buf_count[i]++; //xsens_parse_msg(i); xsens_msg_received[i] = FALSE; } } }
void link_mcu_event_task( void ) { /* A message has been received */ if (InterMcuBuffer()) { while (InterMcuLink(ChAvailable())&&!intermcu_data.msg_available) intermcu_parse(InterMcuLink(Getch())); } if (intermcu_data.msg_available) { parse_mavpilot_msg(); intermcu_data.msg_available = FALSE; } }
void parse_ins_msg( void ) { while (InsLink(ChAvailable())) { uint8_t ch = InsLink(Getch()); if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { if(CHIMU_DATA.m_MsgID==0x03) { new_ins_attitude = 1; RunOnceEvery(25, LED_TOGGLE(3) ); if (CHIMU_DATA.m_attitude.euler.phi > M_PI) { CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; } EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta); //EstimatorSetRate(ins_p,ins_q,ins_r); DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi); } } } }
void parse_ins_msg(void) { while (InsLink(ChAvailable())) { uint8_t ch = InsLink(Getch()); if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { RunOnceEvery(25, LED_TOGGLE(3)); if (CHIMU_DATA.m_MsgID == CHIMU_Msg_3_IMU_Attitude) { new_ins_attitude = 1; if (CHIMU_DATA.m_attitude.euler.phi > M_PI) { CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; } struct FloatEulers att = { CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.theta, CHIMU_DATA.m_attitude.euler.psi }; stateSetNedToBodyEulers_f(&att); struct FloatRates rates = { CHIMU_DATA.m_sensor.rate[0], CHIMU_DATA.m_attrates.euler.theta, 0. }; // FIXME rate r stateSetBodyRates_f(&rates); //FIXME ahrs_chimu.is_aligned = TRUE; } else if (CHIMU_DATA.m_MsgID == 0x02) { #if CHIMU_DOWNLINK_IMMEDIATE RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2])); #endif } } } }
void digital_cam_uart_event(void) { while (CameraLink(ChAvailable())) { parse_mora(&mora_protocol, CameraLink(Getch())); if (mora_protocol.msg_received) { switch (mora_protocol.msg_id) { case MORA_STATUS: for (int i = 0; i < MORA_STATUS_MSG_SIZE; i++) { mora_status_msg.bin[i] = mora_protocol.payload[i]; } digital_cam_uart_status = mora_status_msg.data.shots; break; case MORA_PAYLOAD: for (int i = 0; i < MORA_PAYLOAD_MSG_SIZE; i++) { thumbs[thumb_pointer][i] = mora_protocol.payload[i]; } break; default: break; } mora_protocol.msg_received = 0; } } }
void airspeed_otf_event(void) { while (MetLink(ChAvailable())) { uint8_t ch = MetLink(Getch()); airspeed_otf_parse(ch); } }
void aerocomm_init( void ) { aerocomm_confirmation_received=TRUE; { uint8_t init_cpt = 120; while (init_cpt) { if (sys_time_periodic()) init_cpt--; } } #ifdef NO_API_INIT #warning "NO_API_INIT defined" #else #warning "NO_API_INIT not defined" #endif #ifndef NO_API_INIT /** Switching to AT mode (FIXME: busy waiting) */ AerocommPrintString(AT_COMMAND_SEQUENCE); char c; while (!AerocommLink(ChAvailable())) { uint8_t init_cpt = 3; while (init_cpt) { if (sys_time_periodic()) init_cpt--; } } int j; j=0; while (AerocommLink(ChAvailable()) && j<4) { c=AerocommLink(Getch()); j++; } uint16_t addr = AEROCOMM_MY_ADDR; char s[]=AT_SET_MY; s[4]=(unsigned int)(0x00); s[5]=(unsigned int)(0x50); s[6]=(unsigned int)(0x67); s[7]=(unsigned int)(0x00); s[8]=(unsigned int)(addr>> 8); s[9]=(unsigned int)(addr & 0xff); AerocommPrintString(AT_READ_MY); while (!AerocommLink(ChAvailable())) { uint8_t init_cpt = 3; while (init_cpt) { if (sys_time_periodic()) init_cpt--; } } j=0; while (AerocommLink(ChAvailable()) && j<4) { c=AerocommLink(Getch()); j++; } if (c!=s[9]) for (j=0; j<10; j++) AerocommTransportPut1Byte(s[j]); while (!AerocommLink(ChAvailable())) { uint8_t init_cpt = 3; while (init_cpt) { if (sys_time_periodic()) init_cpt--; } } j=0; while (AerocommLink(ChAvailable()) && j<3) { c=AerocommLink(Getch()); j++; } AerocommPrintString(AT_RESET); { uint8_t init_cpt = 60; while (init_cpt) { if (sys_time_periodic()) init_cpt--; } } AerocommPrintString(AT_COMMAND_SEQUENCE); while (!AerocommLink(ChAvailable())) { uint8_t init_cpt = 3; while (init_cpt) { if (sys_time_periodic()) init_cpt--; } } j=0; while (AerocommLink(ChAvailable()) && j<4) { c=AerocommLink(Getch()); j++; } AerocommPrintString(AT_AP_MODE); while (!AerocommLink(ChAvailable())) { uint8_t init_cpt = 3; while (init_cpt) { if (sys_time_periodic()) init_cpt--; } } j=0; while (AerocommLink(ChAvailable()) && j<2) { c=AerocommLink(Getch()); j++; } char s1[]=AT_DEST_ADDR; s1[2]=(unsigned int)(0x00); s1[3]=(unsigned int)(0x01); s1[4]=(unsigned int)(0x00); for (j=0; j<5; j++) AerocommTransportPut1Byte(s1[j]); while (!AerocommLink(ChAvailable())) { uint8_t init_cpt = 3; while (init_cpt) { if (sys_time_periodic()) init_cpt--; } } j=0; while (AerocommLink(ChAvailable()) && j<4) { c=AerocommLink(Getch()); j++; } AerocommPrintString(AT_EXIT); while (!AerocommLink(ChAvailable())) { uint8_t init_cpt = 3; while (init_cpt) { if (sys_time_periodic()) init_cpt--; } } j=0; while (AerocommLink(ChAvailable()) && j<4) { c=AerocommLink(Getch()); j++; } #endif }