void ins_periodic_task( void ) {
if (xsens_configured > 0)
{
switch (xsens_configured)
{
case 20:
/* send mode and settings to MT */
XSENS_GoToConfig();
XSENS_SetOutputMode(xsens_output_mode);
XSENS_SetOutputSettings(xsens_output_settings);
break;
case 18:
// Give pulses on SyncOut
XSENS_SetSyncOutSettings(0,0x0002);
break;
case 17:
// 1 pulse every 100 samples
XSENS_SetSyncOutSettings(1,100);
break;
case 2:
XSENS_ReqLeverArmGps();
break;
case 6:
XSENS_ReqMagneticDeclination();
break;
case 13:
#ifdef AHRS_H_X
#pragma message "Sending XSens Magnetic Declination."
xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
XSENS_SetMagneticDeclination(xsens_declination);
#endif
break;
case 12:
#ifdef GPS_IMU_LEVER_ARM_X
#pragma message "Sending XSens GPS Arm."
XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
#endif
break;
case 10:
{
uint8_t baud = 1;
XSENS_SetBaudrate(baud);
}
break;
case 1:
XSENS_GoToMeasurment();
break;
default:
break;
}
xsens_configured--;
return;
}
RunOnceEvery(100,XSENS_ReqGPSStatus());
}
void ins_init( void ) {
xsens_status = UNINIT;
xsens_msg_status = 0;
xsens_time_stamp = 0;
xsens_output_mode = XSENS_OUTPUT_MODE;
xsens_output_settings = XSENS_OUTPUT_SETTINGS;
/* send mode and settings to MT */
XSENS_GoToConfig();
XSENS_SetOutputMode(xsens_output_mode);
XSENS_SetOutputSettings(xsens_output_settings);
//XSENS_GoToMeasurment();
}