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 */ }