static inline void update_fw_estimator(void) {
// Send to Estimator (Control)
#ifdef XSENS_BACKWARDS
EstimatorSetAtt((-ins_phi+ins_roll_neutral), (ins_psi + RadOfDeg(180)), (-ins_theta+ins_pitch_neutral));
EstimatorSetRate(-ins_p,-ins_q, ins_r);
#else
EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
EstimatorSetRate(ins_p, ins_q, ins_r);
#endif
}
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 handle_ins_msg( void) {
EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
EstimatorSetRate(ins_p,ins_q);
if (gps_mode != 0x03)
gps_gspeed = 0;
//gps_course = ins_psi * 1800 / M_PI;
gps_course = (DegOfRad(atan2f((float)ins_vx, (float)ins_vy))*10.0f);
gps_climb = (int16_t)(-ins_vz * 100);
gps_gspeed = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy) * 100);
EstimatorSetAtt(ins_phi, RadOfDeg(((float)gps_course) / 10.0), ins_theta);
// EstimatorSetSpeedPol(gps_gspeed, gps_course, ins_vz);
// EstimatorSetAlt(ins_z);
estimator_update_state_gps();
reset_gps_watchdog();
}
void handle_ins_msg( void) {
// Send to Estimator (Control)
#ifdef XSENS_BACKWARDS
EstimatorSetAtt((-ins_phi+ins_roll_neutral), (ins_psi + RadOfDeg(180)), (-ins_theta+ins_pitch_neutral));
EstimatorSetRate(-ins_p,-ins_q);
#else
EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
EstimatorSetRate(ins_p,ins_q);
#endif
// Position
float gps_east = gps.utm_pos.east / 100.;
float gps_north = gps.utm_pos.north / 100.;
gps_east -= nav_utm_east0;
gps_north -= nav_utm_north0;
EstimatorSetPosXY(gps_east, gps_north);
// Altitude and vertical speed
float hmsl = gps.hmsl;
hmsl /= 1000.0f;
EstimatorSetAlt(hmsl);
#ifndef ALT_KALMAN
#warning NO_VZ
#endif
// Horizontal speed
float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
if (gps.fix != GPS_FIX_3D)
{
fspeed = 0;
}
float fclimb = -ins_vz;
float fcourse = atan2f((float)ins_vy, (float)ins_vx);
EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
// Now also finish filling the gps struct for telemetry purposes
gps.gspeed = fspeed * 100.;
gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
gps.course = fcourse * 1e7;
}
void estimator_init( void ) {
EstimatorSetPos (0., 0., 0.);
EstimatorSetAtt (0., 0., 0);
EstimatorSetSpeedPol ( 0., 0., 0.);
EstimatorSetRotSpeed (0., 0., 0.);
estimator_flight_time = 0;
estimator_rad_of_ir = ir_rad_of_ir;
}
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 estimator_init( void ) {
EstimatorSetPosXY(0., 0.);
EstimatorSetAlt(0.);
EstimatorSetAtt (0., 0., 0);
EstimatorSetSpeedPol ( 0., 0., 0.);
EstimatorSetRate(0., 0.);
#ifdef USE_AIRSPEED
EstimatorSetAirspeed( 0. );
#endif
#ifdef USE_AOA
EstimatorSetAOA( 0. );
#endif
estimator_flight_time = 0;
estimator_airspeed = NOMINAL_AIRSPEED;
}
/*
void decode_imupacket( struct imu *data, uint8_t* buffer){
data->phi = (double)((((signed char)buffer[25])<<8)|buffer[26]);
data->theta = (double)((((signed char)buffer[27])<<8)|buffer[28]);
data->psi = (double)((((signed char)buffer[29])<<8)|buffer[30]);
}
*/
void parse_ugear_msg( void ){
float ins_phi, ins_psi, ins_theta;
switch (ugear_type){
case 0: /*gps*/
ugear_debug1 = ugear_debug1+1;
gps_lat = UGEAR_NAV_POSLLH_LAT(ugear_msg_buf);
gps_lon = UGEAR_NAV_POSLLH_LON(ugear_msg_buf);
nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1;
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
gps_utm_east = latlong_utm_x * 100;
gps_utm_north = latlong_utm_y * 100;
gps_alt = UGEAR_NAV_POSLLH_HEIGHT(ugear_msg_buf);
gps_utm_zone = nav_utm_zone0;
gps_gspeed = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
gps_climb = - UGEAR_NAV_POSLLH_VD(ugear_msg_buf);
gps_course = UGEAR_NAV_VELNED_Heading(ugear_msg_buf)/10000; /*in decdegree */
gps_PDOP = UGEAR_NAV_SOL_PDOP(ugear_msg_buf);
gps_Pacc = UGEAR_NAV_SOL_Pacc(ugear_msg_buf);
gps_Sacc = UGEAR_NAV_SOL_Sacc(ugear_msg_buf);
gps_numSV = UGEAR_NAV_SOL_numSV(ugear_msg_buf);
gps_week = 0; // FIXME
gps_itow = UGEAR_NAV_VELNED_ITOW(ugear_msg_buf);
//ugear_debug2 = gps_climb;
//ugear_debug4 = (int32_t)(UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf));
//ugear_debug5 = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
//ugear_debug6 = (int16_t)estimator_phi*100;
gps_mode = 3; /*force GPSfix to be valided*/
gps_pos_available = TRUE; /* The 3 UBX messages are sent in one rafale */
break;
case 1: /*IMU*/
ugear_debug2 = ugear_debug2+1;
ugear_phi = UGEAR_IMU_PHI(ugear_msg_buf);
ugear_psi = UGEAR_IMU_PSI(ugear_msg_buf);
ugear_theta = UGEAR_IMU_THE(ugear_msg_buf);
ugear_debug4 = (int32_t)ugear_phi;
ugear_debug5 = (int32_t)ugear_theta;
ugear_debug6 = (int32_t)ugear_psi;
ugear_debug3 = 333;
ins_phi = (float)ugear_phi/10000 - ins_roll_neutral;
ins_psi = 0;
ins_theta = (float)ugear_theta/10000 - ins_pitch_neutral;
#ifndef INFRARED
EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
#endif
break;
case 2: /*GPS status*/
// ugear_debug1 = 2;
gps_nb_channels = XSENS_GPSStatus_nch(ugear_msg_buf);
uint8_t is;
for(is = 0; is < Min(gps_nb_channels, 16); is++) {
uint8_t ch = XSENS_GPSStatus_chn(ugear_msg_buf,is);
if (ch > 16) continue;
gps_svinfos[ch].svid = XSENS_GPSStatus_svid(ugear_msg_buf, is);
gps_svinfos[ch].flags = XSENS_GPSStatus_bitmask(ugear_msg_buf, is);
gps_svinfos[ch].qi = XSENS_GPSStatus_qi(ugear_msg_buf, is);
gps_svinfos[ch].cno = XSENS_GPSStatus_cnr(ugear_msg_buf, is);
gps_svinfos[ch].elev = 0;
gps_svinfos[ch].azim = 0;
}
break;
case 3: /*servo*/
break;
case 4: /*health*/
break;
}
}
void event_task_fbw( void) {
#ifdef RADIO_CONTROL
if (ppm_valid) {
ppm_valid = FALSE;
radio_control_event_task();
if (rc_values_contains_avg_channels) {
fbw_mode = FBW_MODE_OF_PPRZ(rc_values[RADIO_MODE]);
}
if (fbw_mode == FBW_MODE_MANUAL)
SetCommandsFromRC(commands);
#ifdef CTL_GRZ
if (fbw_mode == FBW_MODE_MANUAL)
ctl_grz_set_setpoints_rate();
/* else if (fbw_mode == FBW_MODE_AUTO) { */
/* SetCommandsFromRC(commands); */
/* } */
#endif /* CTL_GRZ */
}
#endif
#ifdef INTER_MCU
#ifdef MCU_SPI_LINK
if (spi_message_received) {
/* Got a message on SPI. */
spi_message_received = FALSE;
/* Sets link_mcu_received */
/* Sets inter_mcu_received_ap if checksum is ok */
link_mcu_event_task();
}
#endif /* MCU_SPI_LINK */
if (inter_mcu_received_ap) {
inter_mcu_received_ap = FALSE;
inter_mcu_event_task();
if (fbw_mode == FBW_MODE_AUTO) {
SetCommands(ap_state->commands);
}
#ifdef SINGLE_MCU
inter_mcu_fill_fbw_state();
#endif /**Else the buffer is filled even if the last receive was not correct */
}
#ifdef MCU_SPI_LINK
if (link_mcu_received) {
link_mcu_received = FALSE;
inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
link_mcu_restart(); /** Prepares the next SPI communication */
}
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */
#ifdef LINK_IMU
if (spi_message_received) {
/* Got a message on SPI. */
spi_message_received = FALSE;
link_imu_event_task();
EstimatorSetAtt(link_imu_state.eulers[AXIS_X], link_imu_state.eulers[AXIS_Z], link_imu_state.eulers[AXIS_Y]);
#ifdef CTL_GRZ
ctl_grz_set_measures();
#endif /* CTL_GRZ */
}
#endif /* LINK_IMU */
}
