/* * Initialize function loads first data sets, and allocates memory for structure. */ void gps_airspeedInitialize() { // This method saves memory in case we don't use the GPS module. gps = (struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals)); // GPS airspeed calculation variables VelocityStateInitialize(); VelocityStateData gpsVelData; VelocityStateGet(&gpsVelData); gps->gpsVelOld_N = gpsVelData.North; gps->gpsVelOld_E = gpsVelData.East; gps->gpsVelOld_D = gpsVelData.Down; gps->oldAirspeed = 0.0f; AttitudeStateData attData; AttitudeStateGet(&attData); float Rbe[3][3]; float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 }; // Calculate rotation matrix Quaternion2R(q, Rbe); gps->RbeCol1_old[0] = Rbe[0][0]; gps->RbeCol1_old[1] = Rbe[0][1]; gps->RbeCol1_old[2] = Rbe[0][2]; }
/** * @brief initialize UAVOs and structs used by this library */ void plan_initialize() { TakeOffLocationInitialize(); PositionStateInitialize(); PathDesiredInitialize(); FlightModeSettingsInitialize(); FlightStatusInitialize(); AttitudeStateInitialize(); ManualControlCommandInitialize(); VelocityStateInitialize(); VtolPathFollowerSettingsInitialize(); }