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
}
