void GPS_reset_home_position(void) { float Euler[3]; nvtGetEulerRPY(Euler); if (fgps.GPS_FIX && GPS_Info.GPS_numSat >= 5) { GPS_Info.GPS_home[LAT] = GPS_Info.GPS_coord[LAT]; GPS_Info.GPS_home[LON] = GPS_Info.GPS_coord[LON]; GPS_calc_longitude_scaling(GPS_Info.GPS_coord[LAT]); //need an initial value for distance and bearing calc nav_takeoff_bearing = Euler[2];//save takeoff heading //Set ground altitude fgps.GPS_FIX_HOME = 1; } }
void SensorsDynamicCalibrate(char SensorType) { #if STACK_ACC if(SensorType&SENSOR_ACC&&SensorInitState.ACC_Done) { /* TBD */ } #endif #if STACK_GYRO if(SensorType&SENSOR_GYRO&&SensorInitState.GYRO_Done) { if(!SensorCalState.GYRO_Done) { if(nvtGyroCenterCalibrate()!=STATUS_GYRO_CAL_DONE) {} //led_arm_state(LED_STATE_TOGGLE); else { float GyroMean[3]; SensorCalState.GYRO_Done = true; //led_arm_state(LED_STATE_OFF); nvtGetGyroOffset(GyroMean); } } } #endif #if STACK_MAG if(SensorType&SENSOR_MAG&&SensorInitState.MAG_Done) { if(!SensorCalState.MAG_Done) { static float rpy[3],lastY,diff; nvtGetEulerRPY(rpy); diff = fabsf(rpy[2] - lastY); if((diff>0.01f)||(diff==0)) {} //led_mag_state(LED_STATE_TOGGLE); else { //led_arm_state(LED_STATE_OFF); SensorCalState.MAG_Done = true; } lastY = rpy[2]; } } #endif }