/** * Compute desired velocity from the current position * * Takes in @ref PositionActual and compares it to @ref PositionDesired * and computes @ref VelocityDesired */ void updateVtolDesiredVelocity() { static portTickType lastSysTime; portTickType thisSysTime = xTaskGetTickCount();; float dT = 0; GuidanceSettingsData guidanceSettings; PositionActualData positionActual; PositionDesiredData positionDesired; VelocityDesiredData velocityDesired; GuidanceSettingsGet(&guidanceSettings); PositionActualGet(&positionActual); PositionDesiredGet(&positionDesired); VelocityDesiredGet(&velocityDesired); float northError; float eastError; float downError; float northCommand; float eastCommand; float downCommand; // Check how long since last update if(thisSysTime > lastSysTime) // reuse dt in case of wraparound dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; lastSysTime = thisSysTime; // Note all distances in cm // Compute desired north command northError = positionDesired.North - positionActual.North; northPosIntegral = bound(northPosIntegral + northError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI], -guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT], guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]); northCommand = (northError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral); eastError = positionDesired.East - positionActual.East; eastPosIntegral = bound(eastPosIntegral + eastError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI], -guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT], guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]); eastCommand = (eastError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral); velocityDesired.North = bound(northCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax); velocityDesired.East = bound(eastCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax); downError = positionDesired.Down - positionActual.Down; downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI], -guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT], guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]); downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral); velocityDesired.Down = bound(downCommand, -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax); VelocityDesiredSet(&velocityDesired); }
/** * Compute desired velocity from the current position * * Takes in @ref PositionActual and compares it to @ref PositionDesired * and computes @ref VelocityDesired */ void updateVtolDesiredVelocity() { GuidanceSettingsData guidanceSettings; PositionActualData positionActual; PositionDesiredData positionDesired; VelocityDesiredData velocityDesired; GuidanceSettingsGet(&guidanceSettings); PositionActualGet(&positionActual); PositionDesiredGet(&positionDesired); VelocityDesiredGet(&velocityDesired); // Note all distances in cm float dNorth = positionDesired.North - positionActual.North; float dEast = positionDesired.East - positionActual.East; float distance = sqrt(pow(dNorth, 2) + pow(dEast, 2)); float heading = atan2f(dEast, dNorth); float groundspeed = bound(distance * guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_KP], 0, guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX]); velocityDesired.North = groundspeed * cosf(heading); velocityDesired.East = groundspeed * sinf(heading); float dDown = positionDesired.Down - positionActual.Down; velocityDesired.Down = bound(dDown * guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_KP], -guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX], guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX]); VelocityDesiredSet(&velocityDesired); }
/** * Compute desired velocity from the current position and path * * Takes in @ref PositionActual and compares it to @ref PathDesired * and computes @ref VelocityDesired */ static void updatePathVelocity() { float dT = guidanceSettings.UpdatePeriod / 1000.0f; PositionActualData positionActual; PositionActualGet(&positionActual); float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; struct path_status progress; path_progress(&pathDesired, cur, &progress); // Update the path status UAVO PathStatusData pathStatus; PathStatusGet(&pathStatus); pathStatus.fractional_progress = progress.fractional_progress; if (pathStatus.fractional_progress < 1) pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; else pathStatus.Status = PATHSTATUS_STATUS_COMPLETED; PathStatusSet(&pathStatus); float groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress; if(progress.fractional_progress > 1) groundspeed = 0; VelocityDesiredData velocityDesired; velocityDesired.North = progress.path_direction[0] * groundspeed; velocityDesired.East = progress.path_direction[1] * groundspeed; float error_speed = progress.error * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; float correction_velocity[2] = {progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed}; float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2)); float scale = 1; if(total_vel > guidanceSettings.HorizontalVelMax) scale = guidanceSettings.HorizontalVelMax / total_vel; // Currently not apply a PID loop for horizontal corrections velocityDesired.North += progress.correction_direction[0] * error_speed * scale; velocityDesired.East += progress.correction_direction[1] * error_speed * scale; // Interpolate desired velocity along the path float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound_min_max(progress.fractional_progress,0,1); float downError = altitudeSetpoint - positionActual.Down; velocityDesired.Down = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError, -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); VelocityDesiredSet(&velocityDesired); }
/** * Compute desired velocity from the current position * * Takes in @ref PositionActual and compares it to @ref PositionDesired * and computes @ref VelocityDesired */ void updateEndpointVelocity() { float dT = guidanceSettings.UpdatePeriod / 1000.0f; PositionActualData positionActual; VelocityDesiredData velocityDesired; PositionActualGet(&positionActual); VelocityDesiredGet(&velocityDesired); float northError; float eastError; float downError; float northCommand; float eastCommand; float northPos = positionActual.North; float eastPos = positionActual.East; float downPos = positionActual.Down; // Compute desired north command velocity from position error northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos; northCommand = pid_apply_antiwindup(&vtol_pids[NORTH_POSITION], northError, -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); // Compute desired east command velocity from position error eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos; eastCommand = pid_apply_antiwindup(&vtol_pids[EAST_POSITION], eastError, -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); // Limit the maximum velocity any direction (not north and east separately) float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2)); float scale = 1; if(total_vel > guidanceSettings.HorizontalVelMax) scale = guidanceSettings.HorizontalVelMax / total_vel; velocityDesired.North = northCommand * scale; velocityDesired.East = eastCommand * scale; // Compute the desired velocity from the position difference downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos; velocityDesired.Down = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError, -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); VelocityDesiredSet(&velocityDesired); // Indicate whether we are in radius of this endpoint uint8_t path_status = PATHSTATUS_STATUS_INPROGRESS; float distance2 = powf(northError, 2) + powf(eastError, 2); if (distance2 < (guidanceSettings.EndpointRadius * guidanceSettings.EndpointRadius)) { path_status = PATHSTATUS_STATUS_COMPLETED; } PathStatusStatusSet(&path_status); }
/** * Initial position hold at current position. This is used at the end * of a path or in the case of a problem. */ static void holdCurrentPosition() { // TODO: Define a separate error condition method which can select RTH versus PH PositionActualData position; PositionActualGet(&position); PathDesiredData pathDesired; pathDesired.End[PATHDESIRED_END_NORTH] = position.North; pathDesired.End[PATHDESIRED_END_EAST] = position.East; pathDesired.End[PATHDESIRED_END_DOWN] = position.Down; pathDesired.Mode = PATHDESIRED_MODE_HOLDPOSITION; PathDesiredSet(&pathDesired); }
/** * Compute desired velocity from the current position and path * * Takes in @ref PositionActual and compares it to @ref PathDesired * and computes @ref VelocityDesired */ static void updatePathVelocity() { PositionActualData positionActual; PositionActualGet(&positionActual); float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; struct path_status progress; path_progress(&pathDesired, cur, &progress); // Update the path status UAVO PathStatusData pathStatus; PathStatusGet(&pathStatus); pathStatus.fractional_progress = progress.fractional_progress; if (pathStatus.fractional_progress < 1) pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; else pathStatus.Status = PATHSTATUS_STATUS_COMPLETED; PathStatusSet(&pathStatus); float groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress; if(progress.fractional_progress > 1) groundspeed = 0; VelocityDesiredData velocityDesired; velocityDesired.North = progress.path_direction[0] * groundspeed; velocityDesired.East = progress.path_direction[1] * groundspeed; float error_speed = progress.error * guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; float correction_velocity[2] = {progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed}; float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2)); float scale = 1; if(total_vel > guidanceSettings.HorizontalVelMax) scale = guidanceSettings.HorizontalVelMax / total_vel; // Currently not apply a PID loop for horizontal corrections velocityDesired.North += progress.correction_direction[0] * error_speed * scale; velocityDesired.East += progress.correction_direction[1] * error_speed * scale; // No altitude control on a ground vehicle velocityDesired.Down = 0; VelocityDesiredSet(&velocityDesired); }
void PositionActual_Get(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) { if (Param[0]->Val->Pointer == NULL) return; PositionActualData data; PositionActualGet(&data); // use the same struct like picoc. see below struct mystruct { double North; double East; double Down; } *pdata; pdata = Param[0]->Val->Pointer; pdata->North = data.North; pdata->East = data.East; pdata->Down = data.Down; }
/* stub: module has no module thread */ int32_t GeofenceStart(void) { if (geofenceSettings == NULL) { return -1; } // Schedule periodic task to check position UAVObjEvent ev = { .obj = PositionActualHandle(), .instId = 0, .event = 0, }; EventPeriodicCallbackCreate(&ev, checkPosition, SAMPLE_PERIOD_MS); return 0; } MODULE_INITCALL(GeofenceInitialize, GeofenceStart); /** * Periodic callback that processes changes in position and * sets the alarm. */ static void checkPosition(UAVObjEvent* ev, void *ctx, void *obj, int len) { (void) ev; (void) ctx; (void) obj; (void) len; if (PositionActualHandle()) { PositionActualData positionActual; PositionActualGet(&positionActual); const float distance2 = powf(positionActual.North, 2) + powf(positionActual.East, 2); // ErrorRadius is squared when it is fetched, so this is correct if (distance2 > geofenceSettings->ErrorRadius) { AlarmsSet(SYSTEMALARMS_ALARM_GEOFENCE, SYSTEMALARMS_ALARM_ERROR); } else if (distance2 > geofenceSettings->WarningRadius) { AlarmsSet(SYSTEMALARMS_ALARM_GEOFENCE, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_GEOFENCE); } } }
/** * Compute desired attitude from the desired velocity * * Takes in @ref NedActual which has the acceleration in the * NED frame as the feedback term and then compares the * @ref VelocityActual against the @ref VelocityDesired */ uint8_t waypointFollowing(uint8_t flightMode, FixedWingPathFollowerSettingsCCData fixedwingpathfollowerSettings) { float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; //Convert from [ms] to [s] VelocityActualData velocityActual; StabilizationDesiredData stabDesired; float trueAirspeed; float calibratedAirspeedActual; float airspeedDesired; float airspeedError; float pitchCommand; float powerCommand; float headingError_R; float rollCommand; //TODO: Move these out of the loop FixedWingAirspeedsData fixedwingAirspeeds; FixedWingAirspeedsGet(&fixedwingAirspeeds); VelocityActualGet(&velocityActual); StabilizationDesiredGet(&stabDesired); // TODO: Create UAVO that merges airspeed together AirspeedActualTrueAirspeedGet(&trueAirspeed); PositionActualData positionActual; PositionActualGet(&positionActual); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); if (flightStatusUpdate) { //Reset integrals integral->totalEnergyError = 0; integral->airspeedError = 0; integral->lineError = 0; integral->circleError = 0; if (flightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME) { // Simple Return To Home mode: climb 10 meters and fly to home position pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; pathDesired.StartingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.EndingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR; homeOrbit = false; } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) { // Simple position hold: stay at present altitude and position //Offset by one so that the start and end points don't perfectly coincide pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North - 1; pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; pathDesired.StartingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.EndingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR; } PathDesiredSet(&pathDesired); flightStatusUpdate = false; } /** * Compute speed error (required for throttle and pitch) */ // Current airspeed calibratedAirspeedActual = trueAirspeed; //BOOOOOOOOOO!!! Where's the conversion from TAS to CAS? // Current heading float headingActual_R = atan2f(velocityActual.East, velocityActual.North); // Desired airspeed airspeedDesired = pathDesired.EndingVelocity; // Airspeed error airspeedError = airspeedDesired - calibratedAirspeedActual; /** * Compute desired throttle command */ //Proxy because instead of m*(1/2*v^2+g*h), it's v^2+2*gh. This saves processing power float totalEnergyProxySetpoint = powf(pathDesired.EndingVelocity, 2.0f) - 2.0f * GRAVITY * pathDesired.End[2]; float totalEnergyProxyActual = powf(trueAirspeed, 2.0f) - 2.0f * GRAVITY * positionActual.Down; float errorTotalEnergy = totalEnergyProxySetpoint - totalEnergyProxyActual; float throttle_kp = fixedwingpathfollowerSettings. ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_KP]; float throttle_ki = fixedwingpathfollowerSettings.ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_KI]; float throttle_ilimit = fixedwingpathfollowerSettings.ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_ILIMIT]; //Integrate with bound. Make integral leaky for better performance. Approximately 30s time constant. if (throttle_ilimit > 0.0f) { integral->totalEnergyError = bound(integral->totalEnergyError + errorTotalEnergy * dT, -throttle_ilimit / throttle_ki, throttle_ilimit / throttle_ki) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); } powerCommand = errorTotalEnergy * throttle_kp + integral->totalEnergyError * throttle_ki; float throttlelimit_neutral = fixedwingpathfollowerSettings. ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_NEUTRAL]; float throttlelimit_min = fixedwingpathfollowerSettings. ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_MIN]; float throttlelimit_max = fixedwingpathfollowerSettings. ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_MAX]; // set throttle stabDesired.Throttle = bound(powerCommand + throttlelimit_neutral, throttlelimit_min, throttlelimit_max); /** * Compute desired pitch command */ float airspeed_kp = fixedwingpathfollowerSettings. AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_KP]; float airspeed_ki = fixedwingpathfollowerSettings. AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_KI]; float airspeed_ilimit = fixedwingpathfollowerSettings. AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_ILIMIT]; if (airspeed_ki > 0.0f) { //Integrate with saturation integral->airspeedError = bound(integral->airspeedError + airspeedError * dT, -airspeed_ilimit / airspeed_ki, airspeed_ilimit / airspeed_ki); } //Compute the cross feed from altitude to pitch, with saturation float pitchcrossfeed_kp = fixedwingpathfollowerSettings. VerticalToPitchCrossFeed [FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_KP]; float pitchcrossfeed_min = fixedwingpathfollowerSettings. VerticalToPitchCrossFeed [FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_MAX]; float pitchcrossfeed_max = fixedwingpathfollowerSettings. VerticalToPitchCrossFeed [FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_MAX]; float alitudeError = -(pathDesired.End[2] - positionActual.Down); //Negative to convert from Down to altitude float altitudeToPitchCommandComponent = bound(alitudeError * pitchcrossfeed_kp, -pitchcrossfeed_min, pitchcrossfeed_max); //Compute the pitch command as err*Kp + errInt*Ki + X_feed. pitchCommand = -(airspeedError * airspeed_kp + integral->airspeedError * airspeed_ki) + altitudeToPitchCommandComponent; //Saturate pitch command float pitchlimit_neutral = fixedwingpathfollowerSettings. PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_NEUTRAL]; float pitchlimit_min = fixedwingpathfollowerSettings. PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_MIN]; float pitchlimit_max = fixedwingpathfollowerSettings. PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_MAX]; stabDesired.Pitch = bound(pitchlimit_neutral + pitchCommand, pitchlimit_min, pitchlimit_max); /** * Compute desired roll command */ float p[3] = { positionActual.North, positionActual.East, positionActual.Down }; float *c = pathDesired.End; float *r = pathDesired.Start; float q[3] = { pathDesired.End[0] - pathDesired.Start[0], pathDesired.End[1] - pathDesired.Start[1], pathDesired.End[2] - pathDesired.Start[2] }; float k_path = fixedwingpathfollowerSettings.VectorFollowingGain / pathDesired.EndingVelocity; //Divide gain by airspeed so that the turn rate is independent of airspeed float k_orbit = fixedwingpathfollowerSettings.OrbitFollowingGain / pathDesired.EndingVelocity; //Divide gain by airspeed so that the turn rate is independent of airspeed float k_psi_int = fixedwingpathfollowerSettings.FollowerIntegralGain; //======================================== //SHOULD NOT BE HARD CODED bool direction; float chi_inf = PI / 4.0f; //THIS NEEDS TO BE A FUNCTION OF HOW LONG OUR PATH IS. //Saturate chi_inf. I.e., never approach the path at a steeper angle than 45 degrees chi_inf = chi_inf < PI / 4.0f ? PI / 4.0f : chi_inf; //======================================== float rho; float headingCommand_R; float pncn = p[0] - c[0]; float pece = p[1] - c[1]; float d = sqrtf(pncn * pncn + pece * pece); //Assume that we want a 15 degree bank angle. This should yield a nice, non-agressive turn #define ROLL_FOR_HOLDING_CIRCLE 15.0f //Calculate radius, rho, using r*omega=v and omega = g/V_g * tan(phi) //THIS SHOULD ONLY BE CALCULATED ONCE, INSTEAD OF EVERY TIME rho = powf(pathDesired.EndingVelocity, 2) / (GRAVITY * tanf(fabs(ROLL_FOR_HOLDING_CIRCLE * DEG2RAD))); typedef enum { LINE, ORBIT } pathTypes_t; pathTypes_t pathType; //Determine if we should fly on a line or orbit path. switch (flightMode) { case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: if (d < rho + 5.0f * pathDesired.EndingVelocity || homeOrbit == true) { //When approx five seconds from the circle, start integrating into it pathType = ORBIT; homeOrbit = true; } else { pathType = LINE; } break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: pathType = ORBIT; break; default: pathType = LINE; break; } //Check to see if we've gone past our destination. Since the path follower //is simply following a vector, it has no concept of where the vector ends. //It will simply keep following it to infinity if we don't stop it. So while //we don't know why the commutation to the next point failed, we don't know //we don't want the plane flying off. if (pathType == LINE) { //Compute the norm squared of the horizontal path length //IT WOULD BE NICE TO ONLY DO THIS ONCE PER WAYPOINT UPDATE, INSTEAD OF //EVERY LOOP float pathLength2 = q[0] * q[0] + q[1] * q[1]; //Perform a quick vector math operation, |a| < a.b/|a| = |b|cos(theta), //to test if we've gone past the waypoint. Add in a distance equal to 5s //of flight time, for good measure to make sure we don't add jitter. if (((p[0] - r[0]) * q[0] + (p[1] - r[1]) * q[1]) > pathLength2 + 5.0f * pathDesired.EndingVelocity) { //Whoops, we've really overflown our destination point, and haven't //received any instructions. Start circling //flightMode will reset the next time a waypoint changes, so there's //no danger of it getting stuck in orbit mode. flightMode = FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD; pathType = ORBIT; } } switch (pathType) { case ORBIT: if (pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT) { direction = false; } else { //In the case where the direction is undefined, always fly in a //clockwise fashion direction = true; } headingCommand_R = followOrbit(c, rho, direction, p, headingActual_R, k_orbit, k_psi_int, dT); break; case LINE: headingCommand_R = followStraightLine(r, q, p, headingActual_R, chi_inf, k_path, k_psi_int, dT); break; } //Calculate heading error headingError_R = headingCommand_R - headingActual_R; //Wrap heading error around circle if (headingError_R < -PI) headingError_R += 2.0f * PI; if (headingError_R > PI) headingError_R -= 2.0f * PI; //GET RID OF THE RAD2DEG. IT CAN BE FACTORED INTO HeadingPI float rolllimit_neutral = fixedwingpathfollowerSettings. RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_NEUTRAL]; float rolllimit_min = fixedwingpathfollowerSettings. RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_MIN]; float rolllimit_max = fixedwingpathfollowerSettings. RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_MAX]; float headingpi_kp = fixedwingpathfollowerSettings. HeadingPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_HEADINGPI_KP]; rollCommand = (headingError_R * headingpi_kp) * RAD2DEG; //Turn heading stabDesired.Roll = bound(rolllimit_neutral + rollCommand, rolllimit_min, rolllimit_max); #ifdef SIM_OSX fprintf(stderr, " headingError_R: %f, rollCommand: %f\n", headingError_R, rollCommand); #endif /** * Compute desired yaw command */ // Coordinated flight, so we reset the desired yaw. stabDesired.Yaw = 0; stabDesired. StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired. StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired. StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE; StabilizationDesiredSet(&stabDesired); //Stuff some debug variables into PathDesired, because right now these //fields aren't being used. pathDesired.ModeParameters[0] = pitchCommand; pathDesired.ModeParameters[1] = airspeedError; pathDesired.ModeParameters[2] = integral->airspeedError; pathDesired.ModeParameters[3] = alitudeError; pathDesired.UID = errorTotalEnergy; PathDesiredSet(&pathDesired); return 0; }
/** * Telemetry transmit task. Processes queue events and periodic updates. */ static void telemetryRxTask(void *parameters) { uint32_t inputPort; uint8_t c; // Task loop while (1) { #if defined(PIOS_INCLUDE_USB_HID) // Determine input port (USB takes priority over telemetry port) if (PIOS_USB_HID_CheckAvailable(0)) { inputPort = PIOS_COM_TELEM_USB; } else #endif /* PIOS_INCLUDE_USB_HID */ { inputPort = telemetryPort; } mavlink_channel_t mavlink_chan = MAVLINK_COMM_0; // Block until a byte is available PIOS_COM_ReceiveBuffer(inputPort, &c, 1, portMAX_DELAY); // And process it if (mavlink_parse_char(mavlink_chan, c, &rx_msg, &rx_status)) { // Handle packet with waypoint component mavlink_wpm_message_handler(&rx_msg); // Handle packet with parameter component mavlink_pm_message_handler(mavlink_chan, &rx_msg); switch (rx_msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { // Check if this is the gcs mavlink_heartbeat_t beat; mavlink_msg_heartbeat_decode(&rx_msg, &beat); if (beat.type == MAV_TYPE_GCS) { // Got heartbeat from the GCS, we're good! lastOperatorHeartbeat = xTaskGetTickCount() * portTICK_RATE_MS; } } break; case MAVLINK_MSG_ID_SET_MODE: { mavlink_set_mode_t mode; mavlink_msg_set_mode_decode(&rx_msg, &mode); // Check if this system should change the mode if (mode.target_system == mavlink_system.sysid) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); switch (mode.base_mode) { case MAV_MODE_MANUAL_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; case MAV_MODE_MANUAL_DISARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; } break; case MAV_MODE_PREFLIGHT: { flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; } break; case MAV_MODE_STABILIZE_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; case MAV_MODE_GUIDED_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED2; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; case MAV_MODE_AUTO_ARMED: { flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED3; flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED; } break; } bool newHilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL); if (newHilEnabled != hilEnabled) { if (newHilEnabled) { // READ-ONLY flag write to ActuatorCommand UAVObjMetadata meta; UAVObjHandle handle = ActuatorCommandHandle(); UAVObjGetMetadata(handle, &meta); meta.access = ACCESS_READONLY; UAVObjSetMetadata(handle, &meta); mavlink_missionlib_send_gcs_string("ENABLING HIL SIMULATION"); mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++"); mavlink_missionlib_send_gcs_string("BLOCKING ALL ACTUATORS"); } else { // READ-ONLY flag write to ActuatorCommand UAVObjMetadata meta; UAVObjHandle handle = ActuatorCommandHandle(); UAVObjGetMetadata(handle, &meta); meta.access = ACCESS_READWRITE; UAVObjSetMetadata(handle, &meta); mavlink_missionlib_send_gcs_string("DISABLING HIL SIMULATION"); mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++"); mavlink_missionlib_send_gcs_string("ACTIVATING ALL ACTUATORS"); } } hilEnabled = newHilEnabled; FlightStatusSet(&flightStatus); // Check HIL bool hilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL); enableHil(hilEnabled); } } break; case MAVLINK_MSG_ID_HIL_STATE: { if (hilEnabled) { mavlink_hil_state_t hil; mavlink_msg_hil_state_decode(&rx_msg, &hil); // Write GPSPosition GPSPositionData gps; GPSPositionGet(&gps); gps.Altitude = hil.alt/10; gps.Latitude = hil.lat/10; gps.Longitude = hil.lon/10; GPSPositionSet(&gps); // Write PositionActual PositionActualData pos; PositionActualGet(&pos); // FIXME WRITE POSITION HERE PositionActualSet(&pos); // Write AttitudeActual AttitudeActualData att; AttitudeActualGet(&att); att.Roll = hil.roll; att.Pitch = hil.pitch; att.Yaw = hil.yaw; // FIXME //att.RollSpeed = hil.rollspeed; //att.PitchSpeed = hil.pitchspeed; //att.YawSpeed = hil.yawspeed; // Convert to quaternion formulation RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1); // Write AttitudeActual AttitudeActualSet(&att); // Write AttitudeRaw AttitudeRawData raw; AttitudeRawGet(&raw); raw.gyros[0] = hil.rollspeed; raw.gyros[1] = hil.pitchspeed; raw.gyros[2] = hil.yawspeed; raw.accels[0] = hil.xacc; raw.accels[1] = hil.yacc; raw.accels[2] = hil.zacc; // raw.magnetometers[0] = hil.xmag; // raw.magnetometers[0] = hil.ymag; // raw.magnetometers[0] = hil.zmag; AttitudeRawSet(&raw); } } break; case MAVLINK_MSG_ID_COMMAND_LONG: { // FIXME Implement } break; } } } }
/** * Module thread, should not return. */ static void guidanceTask(void *parameters) { SystemSettingsData systemSettings; GuidanceSettingsData guidanceSettings; ManualControlCommandData manualControl; portTickType thisTime; portTickType lastUpdateTime; UAVObjEvent ev; float accel[3] = {0,0,0}; uint32_t accel_accum = 0; float q[4]; float Rbe[3][3]; float accel_ned[3]; // Main task loop lastUpdateTime = xTaskGetTickCount(); while (1) { GuidanceSettingsGet(&guidanceSettings); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(queue, &ev, guidanceSettings.UpdatePeriod / portTICK_RATE_MS) != pdTRUE ) { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); } // Collect downsampled attitude data AttitudeRawData attitudeRaw; AttitudeRawGet(&attitudeRaw); accel[0] += attitudeRaw.accels[0]; accel[1] += attitudeRaw.accels[1]; accel[2] += attitudeRaw.accels[2]; accel_accum++; // Continue collecting data if not enough time thisTime = xTaskGetTickCount(); if( (thisTime - lastUpdateTime) < (guidanceSettings.UpdatePeriod / portTICK_RATE_MS) ) continue; lastUpdateTime = xTaskGetTickCount(); accel[0] /= accel_accum; accel[1] /= accel_accum; accel[2] /= accel_accum; //rotate avg accels into earth frame and store it AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); q[0]=attitudeActual.q1; q[1]=attitudeActual.q2; q[2]=attitudeActual.q3; q[3]=attitudeActual.q4; Quaternion2R(q, Rbe); for (uint8_t i=0; i<3; i++){ accel_ned[i]=0; for (uint8_t j=0; j<3; j++) accel_ned[i] += Rbe[j][i]*accel[j]; } accel_ned[2] += 9.81; NedAccelData accelData; NedAccelGet(&accelData); // Convert from m/s to cm/s accelData.North = accel_ned[0] * 100; accelData.East = accel_ned[1] * 100; accelData.Down = accel_ned[2] * 100; NedAccelSet(&accelData); ManualControlCommandGet(&manualControl); SystemSettingsGet(&systemSettings); GuidanceSettingsGet(&guidanceSettings); if ((manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) && ((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) )) { if(positionHoldLast == 0) { /* When enter position hold mode save current position */ PositionDesiredData positionDesired; PositionActualData positionActual; PositionDesiredGet(&positionDesired); PositionActualGet(&positionActual); positionDesired.North = positionActual.North; positionDesired.East = positionActual.East; PositionDesiredSet(&positionDesired); positionHoldLast = 1; } if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_DUAL_LOOP) updateVtolDesiredVelocity(); else manualSetDesiredVelocity(); updateVtolDesiredAttitude(); } else { // Be cleaner and get rid of global variables northIntegral = 0; eastIntegral = 0; downIntegral = 0; positionHoldLast = 0; } accel[0] = accel[1] = accel[2] = 0; accel_accum = 0; } }
int main() { float gyro[3], accel[3], mag[3]; float vel[3] = { 0, 0, 0 }; /* Normaly we get/set UAVObjects but this one only needs to be set. We will never expect to get this from another module*/ AttitudeActualData attitude_actual; AHRSSettingsData ahrs_settings; /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* Delay system */ PIOS_DELAY_Init(); /* Communication system */ PIOS_COM_Init(); /* ADC system */ AHRS_ADC_Config( adc_oversampling ); /* Setup the Accelerometer FS (Full-Scale) GPIO */ PIOS_GPIO_Enable( 0 ); SET_ACCEL_2G; #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) /* Magnetic sensor system */ PIOS_I2C_Init(); PIOS_HMC5843_Init(); // Get 3 ID bytes strcpy(( char * )mag_data.id, "ZZZ" ); PIOS_HMC5843_ReadID( mag_data.id ); #endif /* SPI link to master */ // PIOS_SPI_Init(); AhrsInitComms(); AHRSCalibrationConnectCallback( calibration_callback ); GPSPositionConnectCallback( gps_callback ); ahrs_state = AHRS_IDLE; while( !AhrsLinkReady() ) { AhrsPoll(); while( ahrs_state != AHRS_DATA_READY ) ; ahrs_state = AHRS_PROCESSING; downsample_data(); ahrs_state = AHRS_IDLE; if(( total_conversion_blocks % 50 ) == 0 ) PIOS_LED_Toggle( LED1 ); } AHRSSettingsGet(&ahrs_settings); /* Use simple averaging filter for now */ for( int i = 0; i < adc_oversampling; i++ ) fir_coeffs[i] = 1; fir_coeffs[adc_oversampling] = adc_oversampling; if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) { // compute a data point and initialize INS downsample_data(); converge_insgps(); } #ifdef DUMP_RAW int previous_conversion; while( 1 ) { AhrsPoll(); int result; uint8_t framing[16] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }; while( ahrs_state != AHRS_DATA_READY ) ; ahrs_state = AHRS_PROCESSING; if( total_conversion_blocks != previous_conversion + 1 ) PIOS_LED_On( LED1 ); // not keeping up else PIOS_LED_Off( LED1 ); previous_conversion = total_conversion_blocks; downsample_data(); ahrs_state = AHRS_IDLE;; // Dump raw buffer result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & valid_data_buffer[0], ADC_OVERSAMPLE * ADC_CONTINUOUS_CHANNELS * sizeof( valid_data_buffer[0] ) ); if( result == 0 ) PIOS_LED_Off( LED1 ); else { PIOS_LED_On( LED1 ); } } #endif timer_start(); /******************* Main EKF loop ****************************/ while( 1 ) { AhrsPoll(); AHRSCalibrationData calibration; AHRSCalibrationGet( &calibration ); BaroAltitudeData baro_altitude; BaroAltitudeGet( &baro_altitude ); GPSPositionData gps_position; GPSPositionGet( &gps_position ); AHRSSettingsGet(&ahrs_settings); // Alive signal if(( total_conversion_blocks % 100 ) == 0 ) PIOS_LED_Toggle( LED1 ); #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) // Get magnetic readings if( PIOS_HMC5843_NewDataAvailable() ) { PIOS_HMC5843_ReadMag( mag_data.raw.axis ); mag_data.updated = 1; } attitude_raw.magnetometers[0] = mag_data.raw.axis[0]; attitude_raw.magnetometers[2] = mag_data.raw.axis[1]; attitude_raw.magnetometers[2] = mag_data.raw.axis[2]; #endif // Delay for valid data counter_val = timer_count(); running_counts = counter_val - last_counter_idle_end; last_counter_idle_start = counter_val; while( ahrs_state != AHRS_DATA_READY ) ; counter_val = timer_count(); idle_counts = counter_val - last_counter_idle_start; last_counter_idle_end = counter_val; ahrs_state = AHRS_PROCESSING; downsample_data(); /***************** SEND BACK SOME RAW DATA ************************/ // Hacky - grab one sample from buffer to populate this. Need to send back // all raw data if it's happening accel_data.raw.x = valid_data_buffer[0]; accel_data.raw.y = valid_data_buffer[2]; accel_data.raw.z = valid_data_buffer[4]; gyro_data.raw.x = valid_data_buffer[1]; gyro_data.raw.y = valid_data_buffer[3]; gyro_data.raw.z = valid_data_buffer[5]; gyro_data.temp.xy = valid_data_buffer[6]; gyro_data.temp.z = valid_data_buffer[7]; if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) { /******************** INS ALGORITHM **************************/ // format data for INS algo gyro[0] = gyro_data.filtered.x; gyro[1] = gyro_data.filtered.y; gyro[2] = gyro_data.filtered.z; accel[0] = accel_data.filtered.x, accel[1] = accel_data.filtered.y, accel[2] = accel_data.filtered.z, // Note: The magnetometer driver returns registers X,Y,Z from the chip which are // (left, backward, up). Remapping to (forward, right, down). mag[0] = -( mag_data.raw.axis[1] - calibration.mag_bias[1] ); mag[1] = -( mag_data.raw.axis[0] - calibration.mag_bias[0] ); mag[2] = -( mag_data.raw.axis[2] - calibration.mag_bias[2] ); INSStatePrediction( gyro, accel, 1 / ( float )EKF_RATE ); INSCovariancePrediction( 1 / ( float )EKF_RATE ); if( gps_updated && gps_position.Status == GPSPOSITION_STATUS_FIX3D ) { // Compute velocity from Heading and groundspeed vel[0] = gps_position.Groundspeed * cos( gps_position.Heading * M_PI / 180 ); vel[1] = gps_position.Groundspeed * sin( gps_position.Heading * M_PI / 180 ); // Completely unprincipled way to make the position variance // increase as data quality decreases but keep it bounded // Variance becomes 40 m^2 and 40 (m/s)^2 when no gps INSSetPosVelVar( 0.004 ); HomeLocationData home; HomeLocationGet( &home ); float ned[3]; double lla[3] = {( double ) gps_position.Latitude / 1e7, ( double ) gps_position.Longitude / 1e7, ( double )( gps_position.GeoidSeparation + gps_position.Altitude )}; // convert from cm back to meters double ecef[3] = {( double )( home.ECEF[0] / 100 ), ( double )( home.ECEF[1] / 100 ), ( double )( home.ECEF[2] / 100 )}; LLA2Base( lla, ecef, ( float( * )[3] ) home.RNE, ned ); if( gps_updated ) { //FIXME: Is this correct? //TOOD: add check for altitude updates FullCorrection( mag, ned, vel, baro_altitude.Altitude ); gps_updated = false; } else { GpsBaroCorrection( ned, vel, baro_altitude.Altitude ); } gps_updated = false; mag_data.updated = 0; } else if( gps_position.Status == GPSPOSITION_STATUS_FIX3D && mag_data.updated == 1 ) { MagCorrection( mag ); // only trust mags if outdoors mag_data.updated = 0; } else { // Indoors, update with zero position and velocity and high covariance INSSetPosVelVar( 0.1 ); vel[0] = 0; vel[1] = 0; vel[2] = 0; VelBaroCorrection( vel, baro_altitude.Altitude ); // MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors } attitude_actual.q1 = Nav.q[0]; attitude_actual.q2 = Nav.q[1]; attitude_actual.q3 = Nav.q[2]; attitude_actual.q4 = Nav.q[3]; } else if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_SIMPLE ) { float q[4]; float rpy[3]; /***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/ /* Very simple computation of the heading and attitude from accel. */ rpy[2] = atan2(( mag_data.raw.axis[0] ), ( -1 * mag_data.raw.axis[1] ) ) * 180 / M_PI; rpy[1] = atan2( accel_data.filtered.x, accel_data.filtered.z ) * 180 / M_PI; rpy[0] = atan2( accel_data.filtered.y, accel_data.filtered.z ) * 180 / M_PI; RPY2Quaternion( rpy, q ); attitude_actual.q1 = q[0]; attitude_actual.q2 = q[1]; attitude_actual.q3 = q[2]; attitude_actual.q4 = q[3]; } ahrs_state = AHRS_IDLE; #ifdef DUMP_FRIENDLY PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "b: %d\r\n", total_conversion_blocks ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "a: %d %d %d\r\n", ( int16_t )( accel_data.filtered.x * 1000 ), ( int16_t )( accel_data.filtered.y * 1000 ), ( int16_t )( accel_data.filtered.z * 1000 ) ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "g: %d %d %d\r\n", ( int16_t )( gyro_data.filtered.x * 1000 ), ( int16_t )( gyro_data.filtered.y * 1000 ), ( int16_t )( gyro_data.filtered.z * 1000 ) ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "m: %d %d %d\r\n", mag_data.raw.axis[0], mag_data.raw.axis[1], mag_data.raw.axis[2] ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "q: %d %d %d %d\r\n", ( int16_t )( Nav.q[0] * 1000 ), ( int16_t )( Nav.q[1] * 1000 ), ( int16_t )( Nav.q[2] * 1000 ), ( int16_t )( Nav.q[3] * 1000 ) ); #endif #ifdef DUMP_EKF uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 }; extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances extern float K[NUMX][NUMV]; // feedback gain matrix // Dump raw buffer int8_t result; result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & mag_data, sizeof( mag_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & gps_data, sizeof( gps_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & accel_data, sizeof( accel_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & gyro_data, sizeof( gyro_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & Q, sizeof( float ) * NUMX * NUMX ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & K, sizeof( float ) * NUMX * NUMV ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & X, sizeof( float ) * NUMX * NUMX ); if( result == 0 ) PIOS_LED_Off( LED1 ); else { PIOS_LED_On( LED1 ); } #endif AttitudeActualSet( &attitude_actual ); /*FIXME: This is dangerous. There is no locking for UAVObjects so it could stomp all over the airspeed/climb rate etc. This used to be done in the OP module which was bad. Having ~4ms latency for the round trip makes it worse here. */ PositionActualData pos; PositionActualGet( &pos ); for( int ct = 0; ct < 3; ct++ ) { pos.NED[ct] = Nav.Pos[ct]; pos.Vel[ct] = Nav.Vel[ct]; } PositionActualSet( &pos ); static bool was_calibration = false; AhrsStatusData status; AhrsStatusGet( &status ); if( was_calibration != status.CalibrationSet ) { was_calibration = status.CalibrationSet; if( status.CalibrationSet ) { calibrate_sensors(); AhrsStatusGet( &status ); status.CalibrationSet = true; } } status.CPULoad = (( float )running_counts / ( float )( idle_counts + running_counts ) ) * 100; status.IdleTimePerCyle = idle_counts / ( TIMER_RATE / 10000 ); status.RunningTimePerCyle = running_counts / ( TIMER_RATE / 10000 ); status.DroppedUpdates = ekf_too_slow; AhrsStatusSet( &status ); } return 0; }
/** * Processes queue events */ static void processObjEvent(UAVObjEvent * ev) { UAVObjMetadata metadata; // FlightTelemetryStatsData flightStats; // GCSTelemetryStatsData gcsTelemetryStatsData; // int32_t retries; // int32_t success; if (ev->obj == 0) { updateTelemetryStats(); } else if (ev->obj == GCSTelemetryStatsHandle()) { gcsTelemetryStatsUpdated(); } else if (ev->obj == TelemetrySettingsHandle()) { updateSettings(); } else { // Get object metadata UAVObjGetMetadata(ev->obj, &metadata); // If this is a metaobject then make necessary telemetry updates if (UAVObjIsMetaobject(ev->obj)) { updateObject(UAVObjGetLinkedObj(ev->obj)); // linked object will be the actual object the metadata are for } mavlink_message_t msg; mavlink_system.sysid = 20; mavlink_system.compid = MAV_COMP_ID_IMU; mavlink_system.type = MAV_TYPE_FIXED_WING; uint8_t mavClass = MAV_AUTOPILOT_OPENPILOT; AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY); // Setup type and object id fields uint32_t objId = UAVObjGetID(ev->obj); // uint64_t timeStamp = 0; switch(objId) { case BAROALTITUDE_OBJID: { BaroAltitudeGet(&baroAltitude); pressure.press_abs = baroAltitude.Pressure*10.0f; pressure.temperature = baroAltitude.Temperature*100.0f; mavlink_msg_scaled_pressure_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &pressure); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case FLIGHTTELEMETRYSTATS_OBJID: { // FlightTelemetryStatsData flightTelemetryStats; FlightTelemetryStatsGet(&flightStats); // XXX this is a hack to make it think it got a confirmed // connection flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED; GCSTelemetryStatsGet(&gcsTelemetryStatsData); gcsTelemetryStatsData.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED; // // // //mavlink_msg_heartbeat_send(MAVLINK_COMM_0,mavlink_system.type,mavClass); // mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, mavlink_system.type, mavClass); // // Copy the message to the send buffer // uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // // Send buffer // PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case SYSTEMSTATS_OBJID: { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); uint8_t system_state = MAV_STATE_UNINIT; uint8_t base_mode = 0; uint8_t custom_mode = 0; // Set flight mode switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_MANUAL: base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: base_mode |= MAV_MODE_FLAG_AUTO_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; default: base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; } // Set arming state switch (flightStatus.Armed) { case FLIGHTSTATUS_ARMED_ARMING: case FLIGHTSTATUS_ARMED_ARMED: system_state = MAV_STATE_ACTIVE; base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; break; case FLIGHTSTATUS_ARMED_DISARMED: system_state = MAV_STATE_STANDBY; base_mode &= !MAV_MODE_FLAG_SAFETY_ARMED; break; } // Set HIL if (hilEnabled) base_mode |= MAV_MODE_FLAG_HIL_ENABLED; mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, mavClass, base_mode, custom_mode, system_state); SystemStatsData stats; SystemStatsGet(&stats); FlightBatteryStateData flightBatteryData; FlightBatteryStateGet(&flightBatteryData); FlightBatterySettingsData flightBatterySettings; FlightBatterySettingsGet(&flightBatterySettings); uint16_t batteryVoltage = (uint16_t)(flightBatteryData.Voltage*1000.0f); int16_t batteryCurrent = -1; // -1: Not present / not estimated int8_t batteryPercent = -1; // -1: Not present / not estimated // if (flightBatterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR] == 0) // { // Factor is zero, sensor is not present // Estimate remaining capacity based on lipo curve batteryPercent = 100.0f*((flightBatteryData.Voltage - 9.6f)/(12.6f - 9.6f)); // } // else // { // // Use capacity and current // batteryPercent = 100.0f*((flightBatterySettings.Capacity - flightBatteryData.ConsumedEnergy) / flightBatterySettings.Capacity); // batteryCurrent = flightBatteryData.Current*100; // } mavlink_msg_sys_status_send(MAVLINK_COMM_0, 0xFF, 0xFF, 0xFF, ((uint16_t)stats.CPULoad*10), batteryVoltage, batteryCurrent, batteryPercent, 0, 0, 0, 0, 0, 0); // // Copy the message to the send buffer // uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // // Send buffer // PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case ATTITUDERAW_OBJID: { AttitudeRawGet(&attitudeRaw); // Copy data attitude_raw.xacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_X]; attitude_raw.yacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Y]; attitude_raw.zacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Z]; attitude_raw.xgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_X]; attitude_raw.ygyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Y]; attitude_raw.zgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Z]; attitude_raw.xmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_X]; attitude_raw.ymag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y]; attitude_raw.zmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z]; mavlink_msg_raw_imu_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &attitude_raw); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); if (hilEnabled) { mavlink_hil_controls_t controls; // Copy data controls.roll_ailerons = 0.1; controls.pitch_elevator = 0.1; controls.yaw_rudder = 0.0; controls.throttle = 0.8; mavlink_msg_hil_controls_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &controls); // Copy the message to the send buffer len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); } break; } case ATTITUDEMATRIX_OBJID: { AttitudeMatrixGet(&attitudeMatrix); // Copy data attitude.roll = attitudeMatrix.Roll; attitude.pitch = attitudeMatrix.Pitch; attitude.yaw = attitudeMatrix.Yaw; attitude.rollspeed = attitudeMatrix.AngularRates[0]; attitude.pitchspeed = attitudeMatrix.AngularRates[1]; attitude.yawspeed = attitudeMatrix.AngularRates[2]; mavlink_msg_attitude_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &attitude); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case GPSPOSITION_OBJID: { GPSPositionGet(&gpsPosition); gps_raw.time_usec = 0; gps_raw.lat = gpsPosition.Latitude*10; gps_raw.lon = gpsPosition.Longitude*10; gps_raw.alt = gpsPosition.Altitude*10; gps_raw.eph = gpsPosition.HDOP*100; gps_raw.epv = gpsPosition.VDOP*100; gps_raw.cog = gpsPosition.Heading*100; gps_raw.satellites_visible = gpsPosition.Satellites; gps_raw.fix_type = gpsPosition.Status; mavlink_msg_gps_raw_int_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &gps_raw); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); // mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps_raw.usec, gps_raw.lat, gps_raw.lon, gps_raw.alt, gps_raw.eph, gps_raw.epv, gps_raw.hdg, gps_raw.satellites_visible, gps_raw.fix_type, 0); break; } case POSITIONACTUAL_OBJID: { PositionActualData pos; PositionActualGet(&pos); mavlink_local_position_ned_t m_pos; m_pos.time_boot_ms = 0; m_pos.x = pos.North; m_pos.y = pos.East; m_pos.z = pos.Down; m_pos.vx = 0.0f; m_pos.vy = 0.0f; m_pos.vz = 0.0f; mavlink_msg_local_position_ned_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &m_pos); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); } break; case ACTUATORCOMMAND_OBJID: { mavlink_rc_channels_scaled_t rc; float val; ManualControlCommandRollGet(&val); rc.chan1_scaled = val*1000; ManualControlCommandPitchGet(&val); rc.chan2_scaled = val*1000; ManualControlCommandYawGet(&val); rc.chan3_scaled = val*1000; ManualControlCommandThrottleGet(&val); rc.chan4_scaled = val*1000; ActuatorCommandData act; ActuatorCommandGet(&act); rc.chan5_scaled = act.Channel[0]; rc.chan6_scaled = act.Channel[1]; rc.chan7_scaled = act.Channel[2]; rc.chan8_scaled = act.Channel[3]; ManualControlCommandData cmd; ManualControlCommandGet(&cmd); rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255; rc.port = 0; mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len); break; } case MANUALCONTROLCOMMAND_OBJID: { mavlink_rc_channels_scaled_t rc; float val; ManualControlCommandRollGet(&val); rc.chan1_scaled = val*1000; ManualControlCommandPitchGet(&val); rc.chan2_scaled = val*1000; ManualControlCommandYawGet(&val); rc.chan3_scaled = val*1000; ManualControlCommandThrottleGet(&val); rc.chan4_scaled = val*1000; rc.chan5_scaled = 0; rc.chan6_scaled = 0; rc.chan7_scaled = 0; rc.chan8_scaled = 0; ManualControlCommandData cmd; ManualControlCommandGet(&cmd); rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255; rc.port = 0; mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len); break; } default: { //printf("unknown object: %x\n",(unsigned int)objId); break; } } } }
static int32_t updateAttitudeComplementary(bool first_run) { UAVObjEvent ev; GyrosData gyrosData; AccelsData accelsData; static int32_t timeval; float dT; static uint8_t init = 0; // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE ) { // When one of these is updated so should the other // Do not set attitude timeout warnings in simulation mode if (!AttitudeActualReadOnly()){ AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING); return -1; } } AccelsGet(&accelsData); // During initialization and FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if(first_run) { #if defined(PIOS_INCLUDE_HMC5883) // To initialize we need a valid mag reading if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE ) return -1; MagnetometerData magData; MagnetometerGet(&magData); #else MagnetometerData magData; magData.x = 100; magData.y = 0; magData.z = 0; #endif AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); init = 0; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); timeval = PIOS_DELAY_GetRaw(); return 0; } if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { // For first 7 seconds use accels to get gyro bias attitudeSettings.AccelKp = 1; attitudeSettings.AccelKi = 0.9; attitudeSettings.YawBiasRate = 0.23; magKp = 1; } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { attitudeSettings.AccelKp = 1; attitudeSettings.AccelKi = 0.9; attitudeSettings.YawBiasRate = 0.23; magKp = 1; init = 0; } else if (init == 0) { // Reload settings (all the rates) AttitudeSettingsGet(&attitudeSettings); magKp = 0.01f; init = 1; } GyrosGet(&gyrosData); // Compute the dT using the cpu clock dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; timeval = PIOS_DELAY_GetRaw(); float q[4]; AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); float grot[3]; float accel_err[3]; // Get the current attitude estimate quat_copy(&attitudeActual.q1, q); // Rotate gravity to body frame and cross with accels grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err); // Account for accel magnitude accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z; accel_mag = sqrtf(accel_mag); accel_err[0] /= accel_mag; accel_err[1] /= accel_mag; accel_err[2] /= accel_mag; if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE ) { // Rotate gravity to body frame and cross with accels float brot[3]; float Rbe[3][3]; MagnetometerData mag; Quaternion2R(q, Rbe); MagnetometerGet(&mag); // If the mag is producing bad data don't use it (normally bad calibration) if (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) { rot_mult(Rbe, homeLocation.Be, brot); float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); mag.x /= mag_len; mag.y /= mag_len; mag.z /= mag_len; float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); brot[0] /= bmag; brot[1] /= bmag; brot[2] /= bmag; // Only compute if neither vector is null if (bmag < 1 || mag_len < 1) mag_err[0] = mag_err[1] = mag_err[2] = 0; else CrossProduct((const float *) &mag.x, (const float *) brot, mag_err); } } else { mag_err[0] = mag_err[1] = mag_err[2] = 0; } // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi; gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi; gyrosBias.z -= mag_err[2] * magKi; GyrosBiasSet(&gyrosBias); // Correct rates based on error, integral component dealt with in updateSensors gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT; gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT; gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2; // Take a time step q[0] = q[0] + qdot[0]; q[1] = q[1] + qdot[1]; q[2] = q[2] + qdot[2]; q[3] = q[3] + qdot[3]; if(q[0] < 0) { q[0] = -q[0]; q[1] = -q[1]; q[2] = -q[2]; q[3] = -q[3]; } // Renomalize qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); q[0] = q[0] / qmag; q[1] = q[1] / qmag; q[2] = q[2] / qmag; q[3] = q[3] / qmag; // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN if((fabs(qmag) < 1.0e-3f) || (qmag != qmag)) { q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } quat_copy(q, &attitudeActual.q1); // Convert into eueler degrees (makes assumptions about RPY order) Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll); AttitudeActualSet(&attitudeActual); // Flush these queues for avoid errors xQueueReceive(baroQueue, &ev, 0); if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) { float NED[3]; // Transform the GPS position into NED coordinates GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); getNED(&gpsPosition, NED); PositionActualData positionActual; PositionActualGet(&positionActual); positionActual.North = NED[0]; positionActual.East = NED[1]; positionActual.Down = NED[2]; PositionActualSet(&positionActual); } if ( xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE ) { // Transform the GPS position into NED coordinates GPSVelocityData gpsVelocity; GPSVelocityGet(&gpsVelocity); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); velocityActual.North = gpsVelocity.North; velocityActual.East = gpsVelocity.East; velocityActual.Down = gpsVelocity.Down; VelocityActualSet(&velocityActual); } AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); return 0; }
/** * Module thread, should not return. */ static void pathfollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; uint32_t lastUpdateTime; AirspeedActualConnectCallback(airspeedActualUpdatedCb); FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); FixedWingAirspeedsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); // Force update of all the settings SettingsUpdatedCb(NULL); FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); path_desired_updated = false; PathDesiredGet(&pathDesired); PathDesiredConnectCallback(pathDesiredUpdated); // Main task loop lastUpdateTime = PIOS_Thread_Systime(); while (1) { // Conditions when this runs: // 1. Must have FixedWing type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) ) { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL); PIOS_Thread_Sleep(1000); continue; } // Continue collecting data if not enough time PIOS_Thread_Sleep_Until(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod); static uint8_t last_flight_mode; FlightStatusGet(&flightStatus); PathStatusGet(&pathStatus); PositionActualData positionActual; static enum {FW_FOLLOWER_IDLE, FW_FOLLOWER_RUNNING, FW_FOLLOWER_ERR} state = FW_FOLLOWER_IDLE; // Check whether an update to the path desired occured and we should // process it. This makes sure that the follower alarm state is // updated. bool process_path_desired_update = (last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER || last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && path_desired_updated; path_desired_updated = false; // Process most of these when the flight mode changes // except when in path following mode in which case // each iteration must make sure this has the latest // PathDesired if (flightStatus.FlightMode != last_flight_mode || process_path_desired_update) { last_flight_mode = flightStatus.FlightMode; switch(flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: state = FW_FOLLOWER_RUNNING; PositionActualGet(&positionActual); pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT; pathDesired.Start[0] = positionActual.North; pathDesired.Start[1] = positionActual.East; pathDesired.Start[2] = positionActual.Down; pathDesired.End[0] = 0; pathDesired.End[1] = 0; pathDesired.End[2] = -30.0f; pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius; pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed; pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed; PathDesiredSet(&pathDesired); break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: state = FW_FOLLOWER_RUNNING; PositionActualGet(&positionActual); pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT; pathDesired.Start[0] = positionActual.North; pathDesired.Start[1] = positionActual.East; pathDesired.Start[2] = positionActual.Down; pathDesired.End[0] = positionActual.North; pathDesired.End[1] = positionActual.East; pathDesired.End[2] = positionActual.Down; pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius; pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed; pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed; PathDesiredSet(&pathDesired); break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL: state = FW_FOLLOWER_RUNNING; PathDesiredGet(&pathDesired); switch(pathDesired.Mode) { case PATHDESIRED_MODE_ENDPOINT: case PATHDESIRED_MODE_VECTOR: case PATHDESIRED_MODE_CIRCLERIGHT: case PATHDESIRED_MODE_CIRCLELEFT: break; default: state = FW_FOLLOWER_ERR; pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; PathStatusSet(&pathStatus); AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL); break; } break; default: state = FW_FOLLOWER_IDLE; break; } } switch(state) { case FW_FOLLOWER_RUNNING: { updatePathVelocity(); uint8_t result = updateFixedDesiredAttitude(); if (result) { AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING); } PathStatusSet(&pathStatus); break; } case FW_FOLLOWER_IDLE: // Be cleaner and get rid of global variables northVelIntegral = 0; eastVelIntegral = 0; downVelIntegral = 0; bearingIntegral = 0; speedIntegral = 0; accelIntegral = 0; powerIntegral = 0; airspeedErrorInt = 0; AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); break; case FW_FOLLOWER_ERR: default: // Leave alarms set above break; } } }
/** * This method is called when a new waypoint is activated * * Note: The way this is called, it runs in an object callback. This is safe because * the execution time is extremely short. If it starts to take a longer time then * the main task look should monitor a flag (such as the waypoint changing) and call * this method from the main task. */ static void activateWaypoint(int idx) { active_waypoint = idx; if (idx >= UAVObjGetNumInstances(WaypointHandle())) { // Attempting to access invalid waypoint. Fall back to position hold at current location AlarmsSet(SYSTEMALARMS_ALARM_PATHPLANNER, SYSTEMALARMS_ALARM_ERROR); holdCurrentPosition(); return; } // Get the activated waypoint WaypointInstGet(idx, &waypoint); PathDesiredData pathDesired; pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; pathDesired.End[PATHDESIRED_END_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN]; pathDesired.ModeParameters = waypoint.ModeParameters; // Use this to ensure the cases match up (catastrophic if not) and to cover any cases // that don't make sense to come from the path planner switch(waypoint.Mode) { case WAYPOINT_MODE_FLYVECTOR: pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR; break; case WAYPOINT_MODE_FLYENDPOINT: pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; break; case WAYPOINT_MODE_FLYCIRCLELEFT: pathDesired.Mode = PATHDESIRED_MODE_FLYCIRCLELEFT; break; case WAYPOINT_MODE_FLYCIRCLERIGHT: pathDesired.Mode = PATHDESIRED_MODE_FLYCIRCLERIGHT; break; case WAYPOINT_MODE_LAND: pathDesired.Mode = PATHDESIRED_MODE_LAND; break; default: holdCurrentPosition(); AlarmsSet(SYSTEMALARMS_ALARM_PATHPLANNER, SYSTEMALARMS_ALARM_ERROR); return; } pathDesired.EndingVelocity = waypoint.Velocity; if(previous_waypoint < 0) { // For first waypoint, get current position as start point PositionActualData positionActual; PositionActualGet(&positionActual); pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 1; pathDesired.StartingVelocity = waypoint.Velocity; } else { // Get previous waypoint as start point WaypointData waypointPrev; WaypointInstGet(previous_waypoint, &waypointPrev); pathDesired.Start[PATHDESIRED_END_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH]; pathDesired.Start[PATHDESIRED_END_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST]; pathDesired.Start[PATHDESIRED_END_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN]; pathDesired.StartingVelocity = waypointPrev.Velocity; } PathDesiredSet(&pathDesired); // Invalidate any pending path status updates path_status_updated = false; AlarmsClear(SYSTEMALARMS_ALARM_PATHPLANNER); }
/** * @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS) * @params[in] first_run This is the first run so trigger reinitialization * @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0) * @return 0 for success, -1 for failure */ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) { UAVObjEvent ev; GyrosData gyrosData; AccelsData accelsData; MagnetometerData magData; BaroAltitudeData baroData; GPSPositionData gpsData; GPSVelocityData gpsVelData; GyrosBiasData gyrosBias; static bool mag_updated = false; static bool baro_updated; static bool gps_updated; static bool gps_vel_updated; static float baroOffset = 0; static uint32_t ins_last_time = 0; static bool inited; float NED[3] = {0.0f, 0.0f, 0.0f}; float vel[3] = {0.0f, 0.0f, 0.0f}; float zeros[3] = {0.0f, 0.0f, 0.0f}; // Perform the update uint16_t sensors = 0; float dT; // Wait until the gyro and accel object is updated, if a timeout then go to failsafe if ( (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) ) { // Do not set attitude timeout warnings in simulation mode if (!AttitudeActualReadOnly()){ AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING); return -1; } } if (inited) { mag_updated = 0; baro_updated = 0; gps_updated = 0; gps_vel_updated = 0; } if (first_run) { inited = false; init_stage = 0; mag_updated = 0; baro_updated = 0; gps_updated = 0; gps_vel_updated = 0; ins_last_time = PIOS_DELAY_GetRaw(); return 0; } mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; // Get most recent data GyrosGet(&gyrosData); AccelsGet(&accelsData); MagnetometerGet(&magData); BaroAltitudeGet(&baroData); GPSPositionGet(&gpsData); GPSVelocityGet(&gpsVelData); GyrosBiasGet(&gyrosBias); // Discard mag if it has NAN (normally from bad calibration) mag_updated &= (magData.x == magData.x && magData.y == magData.y && magData.z == magData.z); // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily // switching between indoor and outdoor mode with Set = false) mag_updated &= (homeLocation.Be[0] != 0 || homeLocation.Be[1] != 0 || homeLocation.Be[2]); // Have a minimum requirement for gps usage gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE); if (!inited) AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); else if (outdoor_mode && gpsData.Satellites < 7) AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); else AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) { // Don't initialize until all sensors are read if (init_stage == 0 && !outdoor_mode) { float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f}; float q[4]; float pos[3] = {0.0f, 0.0f, 0.0f}; // Initialize barometric offset to homelocation altitude baroOffset = -baroData.Altitude; pos[2] = -(baroData.Altitude + baroOffset); // Reset the INS algorithm INSGPSInit(); INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); INSSetBaroVar(revoCalibration.baro_var); // Initialize the gyro bias from the settings float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); MagnetometerGet(&magData); // Set initial attitude AttitudeActualData attitudeActual; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); q[0] = attitudeActual.q1; q[1] = attitudeActual.q2; q[2] = attitudeActual.q3; q[3] = attitudeActual.q4; INSSetState(pos, zeros, q, zeros, zeros); INSResetP(Pdiag); } else if (init_stage == 0 && outdoor_mode) { float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f}; float q[4]; float NED[3]; // Reset the INS algorithm INSGPSInit(); INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); INSSetBaroVar(revoCalibration.baro_var); INSSetMagNorth(homeLocation.Be); // Initialize the gyro bias from the settings float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); // Transform the GPS position into NED coordinates getNED(&gpsPosition, NED); // Initialize barometric offset to cirrent GPS NED coordinate baroOffset = -NED[2] - baroData.Altitude; xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); MagnetometerGet(&magData); // Set initial attitude AttitudeActualData attitudeActual; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); q[0] = attitudeActual.q1; q[1] = attitudeActual.q2; q[2] = attitudeActual.q3; q[3] = attitudeActual.q4; INSSetState(NED, zeros, q, zeros, zeros); INSResetP(Pdiag); } else if (init_stage > 0) { // Run prediction a bit before any corrections dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; GyrosBiasGet(&gyrosBias); float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, (gyrosData.y + gyrosBias.y) * F_PI / 180.0f, (gyrosData.z + gyrosBias.z) * F_PI / 180.0f}; INSStatePrediction(gyros, &accelsData.x, dT); AttitudeActualData attitude; AttitudeActualGet(&attitude); attitude.q1 = Nav.q[0]; attitude.q2 = Nav.q[1]; attitude.q3 = Nav.q[2]; attitude.q4 = Nav.q[3]; Quaternion2RPY(&attitude.q1,&attitude.Roll); AttitudeActualSet(&attitude); } init_stage++; if(init_stage > 10) inited = true; ins_last_time = PIOS_DELAY_GetRaw(); return 0; } if (!inited) return 0; dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; ins_last_time = PIOS_DELAY_GetRaw(); // This should only happen at start up or at mode switches if(dT > 0.01f) dT = 0.01f; else if(dT <= 0.001f) dT = 0.001f; // If the gyro bias setting was updated we should reset // the state estimate of the EKF if(gyroBiasSettingsUpdated) { float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); gyroBiasSettingsUpdated = false; } // Because the sensor module remove the bias we need to add it // back in here so that the INS algorithm can track it correctly float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f}; if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { gyros[0] += gyrosBias.x * F_PI / 180.0f; gyros[1] += gyrosBias.y * F_PI / 180.0f; gyros[2] += gyrosBias.z * F_PI / 180.0f; } // Advance the state estimate INSStatePrediction(gyros, &accelsData.x, dT); // Copy the attitude into the UAVO AttitudeActualData attitude; AttitudeActualGet(&attitude); attitude.q1 = Nav.q[0]; attitude.q2 = Nav.q[1]; attitude.q3 = Nav.q[2]; attitude.q4 = Nav.q[3]; Quaternion2RPY(&attitude.q1,&attitude.Roll); AttitudeActualSet(&attitude); // Advance the covariance estimate INSCovariancePrediction(dT); if(mag_updated) sensors |= MAG_SENSORS; if(baro_updated) sensors |= BARO_SENSOR; INSSetMagNorth(homeLocation.Be); if (gps_updated && outdoor_mode) { INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]); sensors |= POS_SENSORS; if (0) { // Old code to take horizontal velocity from GPS Position update sensors |= HORIZ_SENSORS; vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * F_PI / 180.0f); vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * F_PI / 180.0f); vel[2] = 0; } // Transform the GPS position into NED coordinates getNED(&gpsData, NED); // Track barometric altitude offset with a low pass filter baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + (1.0f - BARO_OFFSET_LOWPASS_ALPHA ) * ( -NED[2] - baroData.Altitude ); } else if (!outdoor_mode) { INSSetPosVelVar(1e2f, 1e2f); vel[0] = vel[1] = vel[2] = 0; NED[0] = NED[1] = 0; NED[2] = -(baroData.Altitude + baroOffset); sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; sensors |= POS_SENSORS |VERT_SENSORS; } if (gps_vel_updated && outdoor_mode) { sensors |= HORIZ_SENSORS | VERT_SENSORS; vel[0] = gpsVelData.North; vel[1] = gpsVelData.East; vel[2] = gpsVelData.Down; } /* * TODO: Need to add a general sanity check for all the inputs to make sure their kosher * although probably should occur within INS itself */ if (sensors) INSCorrection(&magData.x, NED, vel, ( baroData.Altitude + baroOffset ), sensors); // Copy the position and velocity into the UAVO PositionActualData positionActual; PositionActualGet(&positionActual); positionActual.North = Nav.Pos[0]; positionActual.East = Nav.Pos[1]; positionActual.Down = Nav.Pos[2]; PositionActualSet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); velocityActual.North = Nav.Vel[0]; velocityActual.East = Nav.Vel[1]; velocityActual.Down = Nav.Vel[2]; VelocityActualSet(&velocityActual); if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) { // Copy the gyro bias into the UAVO except when it was updated // from the settings during the calculation, then consume it // next cycle gyrosBias.x = Nav.gyro_bias[0] * 180.0f / F_PI; gyrosBias.y = Nav.gyro_bias[1] * 180.0f / F_PI; gyrosBias.z = Nav.gyro_bias[2] * 180.0f / F_PI; GyrosBiasSet(&gyrosBias); } return 0; }
/** * Compute desired velocity from the current position and path * * Takes in @ref PositionActual and compares it to @ref PathDesired * and computes @ref VelocityDesired */ static void updatePathVelocity() { PositionActualData positionActual; PositionActualGet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; struct path_status progress; path_progress(&pathDesired, cur, &progress); float groundspeed = 0; float altitudeSetpoint = 0; switch (pathDesired.Mode) { case PATHDESIRED_MODE_CIRCLERIGHT: case PATHDESIRED_MODE_CIRCLELEFT: groundspeed = pathDesired.EndingVelocity; altitudeSetpoint = pathDesired.End[2]; break; case PATHDESIRED_MODE_ENDPOINT: case PATHDESIRED_MODE_VECTOR: default: groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound_min_max(progress.fractional_progress,0,1); altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound_min_max(progress.fractional_progress,0,1); break; } // this ensures a significant forward component at least close to the real trajectory if (groundspeed<fixedWingAirspeeds.BestClimbRateSpeed/10.0f) groundspeed=fixedWingAirspeeds.BestClimbRateSpeed/10.0f; // calculate velocity - can be zero if waypoints are too close VelocityDesiredData velocityDesired; velocityDesired.North = progress.path_direction[0] * groundspeed; velocityDesired.East = progress.path_direction[1] * groundspeed; float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP; // calculate correction - can also be zero if correction vector is 0 or no error present velocityDesired.North += progress.correction_direction[0] * error_speed; velocityDesired.East += progress.correction_direction[1] * error_speed; float downError = altitudeSetpoint - positionActual.Down; velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; // update pathstatus pathStatus.error = progress.error; pathStatus.fractional_progress = progress.fractional_progress; pathStatus.fractional_progress = progress.fractional_progress; if (pathStatus.fractional_progress < 1) pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; else pathStatus.Status = PATHSTATUS_STATUS_COMPLETED; pathStatus.Waypoint = pathDesired.Waypoint; VelocityDesiredSet(&velocityDesired); }
/** * Compute desired velocity to follow the desired path from the current location. * @param[in] dT the time since last evaluation * @param[in] pathDesired the desired path to follow * @param[out] progress the current progress information along that path * @returns 0 if successful, <0 if an error occurred * * The calculated velocity to attempt is stored in @ref VelocityDesired */ int32_t vtol_follower_control_path(const float dT, const PathDesiredData *pathDesired, struct path_status *progress) { PositionActualData positionActual; PositionActualGet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); PathStatusData pathStatus; PathStatusGet(&pathStatus); const float cur_pos_ned[3] = { positionActual.North + velocityActual.North * guidanceSettings.PositionFeedforward, positionActual.East + velocityActual.East * guidanceSettings.PositionFeedforward, positionActual.Down }; path_progress(pathDesired, cur_pos_ned, progress); // Check if we have already completed this leg bool current_leg_completed = (pathStatus.Status == PATHSTATUS_STATUS_COMPLETED) && (pathStatus.Waypoint == pathDesired->Waypoint); pathStatus.fractional_progress = progress->fractional_progress; pathStatus.error = progress->error; pathStatus.Waypoint = pathDesired->Waypoint; // Figure out how low (high) we should be and the error const float altitudeSetpoint = vtol_interpolate(progress->fractional_progress, pathDesired->Start[2], pathDesired->End[2]); const float downError = altitudeSetpoint - positionActual.Down; // If leg is completed signal this if (current_leg_completed || pathStatus.fractional_progress > 1.0f) { const bool criterion_altitude = (downError > -guidanceSettings.WaypointAltitudeTol) || (!guidanceSettings.ThrottleControl); // Once we complete leg and hit altitude criterion signal this // waypoint is done. Or if we're not controlling throttle, // ignore height for completion. // Waypoint heights are thus treated as crossing restrictions- // cross this point at or above... if (criterion_altitude || current_leg_completed) { pathStatus.Status = PATHSTATUS_STATUS_COMPLETED; PathStatusSet(&pathStatus); } else { pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; PathStatusSet(&pathStatus); } // Wait here for new path segment return vtol_follower_control_simple(dT, pathDesired->End, false, false); } // Interpolate desired velocity and altitude along the path float groundspeed = vtol_interpolate(progress->fractional_progress, pathDesired->StartingVelocity, pathDesired->EndingVelocity); float error_speed = vtol_deadband(progress->error, guidanceSettings.PathDeadbandWidth, guidanceSettings.PathDeadbandCenterGain, vtol_path_m, vtol_path_r) * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; /* Sum the desired path movement vector with the correction vector */ float commands_ned[3]; commands_ned[0] = progress->path_direction[0] * groundspeed + progress->correction_direction[0] * error_speed; commands_ned[1] = progress->path_direction[1] * groundspeed + progress->correction_direction[1] * error_speed; /* Limit the total velocity based on the configured value. */ vtol_limit_velocity(commands_ned, guidanceSettings.HorizontalVelMax); commands_ned[2] = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError, -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); VelocityDesiredData velocityDesired; VelocityDesiredGet(&velocityDesired); velocityDesired.North = commands_ned[0]; velocityDesired.East = commands_ned[1]; velocityDesired.Down = commands_ned[2]; VelocityDesiredSet(&velocityDesired); pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; PathStatusSet(&pathStatus); return 0; }
/** * Controller to maintain/seek a position and optionally descend. * @param[in] dT time since last eval * @param[in] hold_pos_ned a position to hold * @param[in] landing whether to descend * @param[in] update_status does this update path_status, or does somoene else? */ static int32_t vtol_follower_control_simple(const float dT, const float *hold_pos_ned, bool landing, bool update_status) { PositionActualData positionActual; VelocityDesiredData velocityDesired; PositionActualGet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); /* Where would we be in ___ second at current rates? */ const float cur_pos_ned[3] = { positionActual.North + velocityActual.North * guidanceSettings.PositionFeedforward, positionActual.East + velocityActual.East * guidanceSettings.PositionFeedforward, positionActual.Down }; float errors_ned[3]; /* Calculate the difference between where we want to be and the * above position */ vtol_calculate_difference(cur_pos_ned, hold_pos_ned, errors_ned, false); float horiz_error_mag = vtol_magnitude(errors_ned, 2); float scale_horiz_error_mag = 0; /* Apply a cubic deadband; if we are already really close don't work * as hard to fix it as if we're far away. Prevents chasing high * frequency noise in direction of correction. * * That is, when we're far, noise in estimated position mostly results * in noise/dither in how fast we go towards the target. When we're * close, there's a large directional / hunting component. This * attenuates that. */ if (horiz_error_mag > 0.00001f) { scale_horiz_error_mag = vtol_deadband(horiz_error_mag, guidanceSettings.EndpointDeadbandWidth, guidanceSettings.EndpointDeadbandCenterGain, vtol_end_m, vtol_end_r) / horiz_error_mag; } float damped_ne[2] = { errors_ned[0] * scale_horiz_error_mag, errors_ned[1] * scale_horiz_error_mag }; float commands_ned[3]; // Compute desired north command velocity from position error commands_ned[0] = pid_apply_antiwindup(&vtol_pids[NORTH_POSITION], damped_ne[0], -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); // Compute desired east command velocity from position error commands_ned[1] = pid_apply_antiwindup(&vtol_pids[EAST_POSITION], damped_ne[1], -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); if (!landing) { // Compute desired down comand velocity from the position difference commands_ned[2] = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], errors_ned[2], -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); } else { // Just use the landing rate. commands_ned[2] = guidanceSettings.LandingRate; } // Limit the maximum horizontal velocity any direction (not north and east separately) vtol_limit_velocity(commands_ned, guidanceSettings.HorizontalVelMax); velocityDesired.North = commands_ned[0]; velocityDesired.East = commands_ned[1]; velocityDesired.Down = commands_ned[2]; VelocityDesiredSet(&velocityDesired); if (update_status) { uint8_t path_status = PATHSTATUS_STATUS_INPROGRESS; if (!landing) { const bool criterion_altitude = (errors_ned[2]> -guidanceSettings.WaypointAltitudeTol) || (!guidanceSettings.ThrottleControl); // Indicate whether we are in radius of this endpoint // And at/above the altitude requested if ((vtol_magnitude(errors_ned, 2) < guidanceSettings.EndpointRadius) && criterion_altitude) { path_status = PATHSTATUS_STATUS_COMPLETED; } } // landing never terminates. PathStatusStatusSet(&path_status); } return 0; }
bool vtol_follower_control_loiter(float dT, float *hold_pos, float *att_adj, float *alt_adj) { LoiterCommandData cmd; LoiterCommandGet(&cmd); // XXX TODO reproject when we're not issuing body-centric commands float commands_rp[2] = { cmd.Roll, cmd.Pitch }; const float CMD_THRESHOLD = 0.2f; float command_mag = vectorn_magnitude(commands_rp, 2); float deadband_mag = loiter_deadband(command_mag, CMD_THRESHOLD, 40); float down_cmd = 0; if (guidanceSettings.ThrottleControl && guidanceSettings.LoiterAllowAltControl) { // Inverted because we want units in "Down" frame // Doubled to recenter to 1 to -1 scale from 0-1. // loiter_deadband clips appropriately. down_cmd = loiter_deadband(1 - (cmd.Throttle * 2), altitudeHoldSettings.Deadband / 100.0f, altitudeHoldSettings.Expo); } // Peak detect and decay of the past command magnitude static float historic_mag = 0.0f; // Initialize altitude command *alt_adj = 0; // First reduce by decay constant historic_mag *= loiter_brakealpha; // If our current magnitude is greater than the result, increase it. if (deadband_mag > historic_mag) { historic_mag = deadband_mag; } // And if we haven't had any significant command lately, bug out and // do nothing. if ((historic_mag < 0.001f) && (fabsf(down_cmd) < 0.001f)) { att_adj[0] = 0; att_adj[1] = 0; return false; } // Normalize our command magnitude. Command vectors from this // point are normalized. if (command_mag >= 0.001f) { commands_rp[0] /= command_mag; commands_rp[1] /= command_mag; } else { // Just pick a direction commands_rp[0] = 0.0f; commands_rp[1] = -1.0f; } // Find our current position error PositionActualData positionActual; PositionActualGet(&positionActual); float cur_pos_ned[3] = { positionActual.North, positionActual.East, positionActual.Down }; float total_poserr_ned[3]; vector3_distances(cur_pos_ned, hold_pos, total_poserr_ned, false); if (deadband_mag >= 0.001f) { float yaw; AttitudeActualYawGet(&yaw); float commands_ne[2]; // 90 degrees here compensates for the above being in roll-pitch // order vs. north-east (and where yaw is defined). vector2_rotate(commands_rp, commands_ne, 90 + yaw); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); // Come up with a target velocity for us to fly the command // at, considering our current momentum in that direction. float target_vel = guidanceSettings.LoiterInitialMaxVel * deadband_mag; // Plus whatever current velocity we're making good in // that direction.. // find the portion of our current velocity vector parallel to // cmd. float parallel_sign = velocityActual.North * commands_ne[0] + velocityActual.East * commands_ne[1]; if (parallel_sign > 0) { float parallel_mag = sqrtf( powf(velocityActual.North * commands_ne[0], 2) + powf(velocityActual.East * commands_ne[1], 2)); target_vel += deadband_mag * parallel_mag; } // Feed the target velocity forward for our new desired position hold_pos[0] = cur_pos_ned[0] + commands_ne[0] * target_vel * guidanceSettings.LoiterLookaheadTimeConstant; hold_pos[1] = cur_pos_ned[1] + commands_ne[1] * target_vel * guidanceSettings.LoiterLookaheadTimeConstant; } // Now put a portion of the error back in. At full stick // deflection, decay error at specified time constant float scaled_error_alpha = 1 - historic_mag * (1 - loiter_errordecayalpha); hold_pos[0] -= scaled_error_alpha * total_poserr_ned[0]; hold_pos[1] -= scaled_error_alpha * total_poserr_ned[1]; // Compute attitude feedforward att_adj[0] = deadband_mag * commands_rp[0] * guidanceSettings.LoiterAttitudeFeedthrough; att_adj[1] = deadband_mag * commands_rp[1] * guidanceSettings.LoiterAttitudeFeedthrough; // If we are being commanded to climb or descend... if (fabsf(down_cmd) >= 0.001f) { // Forgive all altitude error so when position controller comes // back we do something sane hold_pos[2] = cur_pos_ned[2]; // and output an adjustment for velocity control use */ *alt_adj = down_cmd * guidanceSettings.VerticalVelMax; } return true; }