void VtolLandFSM::run_at_descentrate(uint8_t flTimeout)
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mLandData->sum1 += velocityState.Down;
mLandData->sum2 += stabDesired.Thrust;
mLandData->observationCount++;
if (flTimeout) {
mLandData->fsmLandStatus.averageDescentRate = boundf((mLandData->sum1 / (float)(mLandData->observationCount)), 0.5f * MIN_LANDRATE, 1.5f * MAX_LANDRATE);
mLandData->fsmLandStatus.averageDescentThrust = boundf((mLandData->sum2 / (float)(mLandData->observationCount)), vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
// We need to calculate a neutral limit to use later to constrain upper thrust range during states where we are close to the ground
// As our battery gets flat, ThrustLimits.Neutral needs to constrain us too much and we get too fast a descent rate. We can
// detect this by the fact that the descent rate will exceed the target and the required thrust will exceed the neutral value
mLandData->fsmLandStatus.calculatedNeutralThrust = mLandData->fsmLandStatus.averageDescentRate / mLandData->fsmLandStatus.targetDescentRate * mLandData->fsmLandStatus.averageDescentThrust;
mLandData->fsmLandStatus.calculatedNeutralThrust = boundf(mLandData->fsmLandStatus.calculatedNeutralThrust, vtolPathFollowerSettings->ThrustLimits.Neutral, vtolPathFollowerSettings->ThrustLimits.Max);
setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
}
}
static bool normalizeDeadband(float controlVector[4])
{
bool moving = false;
// roll, pitch, yaw between -1 and +1
// thrust between 0 and 1 mapped to -1 to +1
controlVector[3] = (2.0f * controlVector[3]) - 1.0f;
int t;
for (t = 0; t < 4; t++) {
if (controlVector[t] < -DEADBAND) {
moving = true;
controlVector[t] += DEADBAND;
} else if (controlVector[t] > DEADBAND) {
moving = true;
controlVector[t] -= DEADBAND;
} else {
controlVector[t] = 0.0f;
}
// deadband has been cut out, scale value back to [-1,+1]
controlVector[t] *= (1.0f / (1.0f - DEADBAND));
controlVector[t] = boundf(controlVector[t], -1.0f, 1.0f);
}
return moving;
}
static float applyExpo(float value, float expo)
{
// note: fastPow makes a small error, therefore result needs to be bound
float exp = boundf(fastPow(1.00695f, expo), 0.5f, 2.0f);
// magic number scales expo
// so that
// expo=100 yields value**10
// expo=0 yields value**1
// expo=-100 yields value**(1/10)
// (pow(2.0,1/100)~=1.00695)
if (value > 0.0f) {
return boundf(fastPow(value, exp), 0.0f, 1.0f);
} else if (value < -0.0f) {
return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
} else {
return 0.0f;
}
}
float VtolLandFSM::BoundVelocityDown(float velocity_down)
{
velocity_down = boundf(velocity_down, MIN_LANDRATE, MAX_LANDRATE);
if (mLandData->flLowAltitude) {
velocity_down *= LOW_ALT_DESCENT_REDUCTION_FACTOR;
}
mLandData->fsmLandStatus.targetDescentRate = velocity_down;
if (mLandData->flAltitudeHold) {
return 0.0f;
} else {
return velocity_down;
}
}
/**
* @brief Compute progress along path and deviation from it
* @param[in] path PathDesired
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
* @param[in] mode3D set true to include altitude in distance and progress calculation
*/
static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
{
float diff[3];
float dist_path;
float dot;
float velocity;
float track_point[3];
// Distance to go
status->path_vector[0] = path->End.North - path->Start.North;
status->path_vector[1] = path->End.East - path->Start.East;
status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f;
// Current progress location relative to start
diff[0] = cur_point[0] - path->Start.North;
diff[1] = cur_point[1] - path->Start.East;
diff[2] = mode3D ? cur_point[2] - path->Start.Down : 0.0f;
dot = status->path_vector[0] * diff[0] + status->path_vector[1] * diff[1] + status->path_vector[2] * diff[2];
dist_path = vector_lengthf(status->path_vector, 3);
if (dist_path > 1e-6f) {
// Compute direction to travel & progress
status->fractional_progress = dot / (dist_path * dist_path);
} else {
// Fly towards the endpoint to prevent flying away,
// but assume progress=1 either way.
path_endpoint(path, cur_point, status, mode3D);
status->fractional_progress = 1;
return;
}
// Compute point on track that is closest to our current position.
track_point[0] = status->fractional_progress * status->path_vector[0] + path->Start.North;
track_point[1] = status->fractional_progress * status->path_vector[1] + path->Start.East;
track_point[2] = status->fractional_progress * status->path_vector[2] + path->Start.Down;
status->correction_vector[0] = track_point[0] - cur_point[0];
status->correction_vector[1] = track_point[1] - cur_point[1];
status->correction_vector[2] = track_point[2] - cur_point[2];
status->error = vector_lengthf(status->correction_vector, 3);
// correct movement vector to current velocity
velocity = path->StartingVelocity + boundf(status->fractional_progress, 0.0f, 1.0f) * (path->EndingVelocity - path->StartingVelocity);
status->path_vector[0] = velocity * status->path_vector[0] / dist_path;
status->path_vector[1] = velocity * status->path_vector[1] / dist_path;
status->path_vector[2] = velocity * status->path_vector[2] / dist_path;
}
/**
* Compute desired attitude for vtols - emergency fallback
*/
void VtolFlyController::UpdateDesiredAttitudeEmergencyFallback()
{
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
float courseError;
float courseCommand;
VelocityStateGet(&velocityState);
VelocityDesiredGet(&velocityDesired);
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
courseCommand = (courseError * vtolPathFollowerSettings->EmergencyFallbackYawRate.kP);
stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings->EmergencyFallbackYawRate.Max, vtolPathFollowerSettings->EmergencyFallbackYawRate.Max);
controlDown.UpdateVelocitySetpoint(velocityDesired.Down);
controlDown.UpdateVelocityState(velocityState.Down);
stabDesired.Thrust = controlDown.GetDownCommand();
stabDesired.Roll = vtolPathFollowerSettings->EmergencyFallbackAttitude.Roll;
stabDesired.Pitch = vtolPathFollowerSettings->EmergencyFallbackAttitude.Pitch;
if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
stabDesired.Thrust = manualControlData.Thrust;
}
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
StabilizationDesiredSet(&stabDesired);
}
/**
* Update the PID computation
* @param[in] pid The PID struture which stores temporary information
* @param[in] err The error term
* @param[in] dT The time step
* @returns Output the computed controller value
*/
float pid_apply(struct pid *pid, const float err, float dT)
{
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
// Calculate DT1 term
float diff = (err - pid->lastErr);
float dterm = 0;
pid->lastErr = err;
if (pid->d > 0.0f && dT > 0.0f) {
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
} // 7.9577e-3 means 20 Hz f_cutoff
return (err * pid->p) + pid->iAccumulator / 1000.0f + dterm;
}
/**
* Update the PID computation with setpoint weighting on the derivative
* @param[in] pid The PID struture which stores temporary information
* @param[in] factor A dynamic factor to scale pid's by, to compensate nonlinearities
* @param[in] setpoint The setpoint to use
* @param[in] measured The measured value of output
* @param[in] dT The time step
* @returns Output the computed controller value
*
* This version of apply uses setpoint weighting for the derivative component so the gain
* on the gyro derivative can be different than the gain on the setpoint derivative
*/
float pid_apply_setpoint(struct pid *pid, const pid_scaler *scaler, const float setpoint, const float measured, float dT)
{
float err = setpoint - measured;
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (scaler->i * pid->i * dT * 1000.0f);
pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
// Calculate DT1 term,
float dterm = 0;
float diff = ((deriv_gamma * setpoint - measured) - pid->lastErr);
pid->lastErr = (deriv_gamma * setpoint - measured);
if (pid->d > 0.0f && dT > 0.0f) {
// low pass filter derivative term. below formula is the same as
// dterm = (1-alpha)*pid->lastDer + alpha * (...)/dT
// with alpha = dT/(deriv_tau+dT)
dterm = pid->lastDer + dT / (dT + deriv_tau) * ((scaler->d * diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm;
}
return (err * scaler->p * pid->p) + pid->iAccumulator / 1000.0f + dterm;
}
int8_t VtolFlyController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
// TODO The below need to be rewritten because the PID implementation has changed.
#if 0
// DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
attitudeState.Yaw += 120.0f;
if (attitudeState.Yaw > 180.0f) {
attitudeState.Yaw -= 360.0f;
}
}
if ( // emergency flyaway detection
( // integral already at its limit
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
) &&
// angle between desired and actual velocity >90 degrees (by dot product)
(velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
// quad is moving at significant speed (during flyaway it would keep speeding up)
squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
) {
vtolEmergencyFallback += dT;
if (vtolEmergencyFallback >= vtolPathFollowerSettings->FlyawayEmergencyFallbackTriggerTime) {
// after emergency timeout, trigger alarm - everything else is handled by callers
// (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
result = 0;
}
} else {
vtolEmergencyFallback = 0.0f;
}
#endif // if 0
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
if (mManualThrust) {
stabDesired.Thrust = manualControl.Thrust;
} else {
stabDesired.Thrust = controlDown.GetDownCommand();
}
StabilizationDesiredSet(&stabDesired);
return result;
}
/**
* @brief execute autocruise
*/
void plan_run_AutoCruise()
{
PositionStateData positionState;
PositionStateGet(&positionState);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
FlightModeSettingsPositionHoldOffsetData offset;
FlightModeSettingsPositionHoldOffsetGet(&offset);
float controlVector[4];
ManualControlCommandRollGet(&controlVector[0]);
ManualControlCommandPitchGet(&controlVector[1]);
ManualControlCommandYawGet(&controlVector[2]);
controlVector[3] = 0.5f; // dummy, thrust is normalized separately
normalizeDeadband(controlVector); // return value ignored
ManualControlCommandThrustGet(&controlVector[3]); // no deadband as we are using thrust for velocity
controlVector[3] = boundf(controlVector[3], 1e-6f, 1.0f); // bound to above zero, to prevent loss of vector direction
// normalize old desired movement vector
float vector[3] = { pathDesired.End.North - hold_position[0],
pathDesired.End.East - hold_position[1],
pathDesired.End.Down - hold_position[2] };
float length = sqrtf(vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]);
if (length < 1e-9f) {
length = 1.0f; // should not happen since initialized properly in setup()
}
vector[0] /= length;
vector[1] /= length;
vector[2] /= length;
// start position is advanced according to actual movement - in the direction of desired vector only
// projection using scalar product
float kp = (positionState.North - hold_position[0]) * vector[0]
+ (positionState.East - hold_position[1]) * vector[1]
+ (positionState.Down - hold_position[2]) * vector[2];
if (kp > 0.0f) {
hold_position[0] += kp * vector[0];
hold_position[1] += kp * vector[1];
hold_position[2] += kp * vector[2];
}
// new angle is equal to old angle plus offset depending on yaw input and time
// (controlVector is normalized with a deadband, change is zero within deadband)
float angle = RAD2DEG(atan2f(vector[1], vector[0]));
float dT = PIOS_DELTATIME_GetAverageSeconds(&actimeval);
angle += 10.0f * controlVector[2] * dT; // TODO magic value could eventually end up in a to be created settings
// resulting movement vector is scaled by velocity demand in controlvector[3] [0.0-1.0]
vector[0] = cosf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
vector[1] = sinf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
vector[2] = -controlVector[1] * offset.Vertical * controlVector[3];
pathDesired.End.North = hold_position[0] + vector[0];
pathDesired.End.East = hold_position[1] + vector[1];
pathDesired.End.Down = hold_position[2] + vector[2];
// start position has the same offset as in position hold
pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
pathDesired.Start.East = pathDesired.End.East;
pathDesired.Start.Down = pathDesired.End.Down;
PathDesiredSet(&pathDesired);
}
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->VelocityRoamMaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
// default thrust mode to altvario
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO;
stabDesired.Thrust = manualControl.Thrust;
StabilizationDesiredSet(&stabDesired);
/**
* Compute desired attitude from the desired velocity for fixed wing craft
*/
uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
{
uint8_t result = 1;
const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
AirspeedStateData airspeedState;
SystemSettingsData systemSettings;
float groundspeedProjection;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
float pitchCommand;
float descentspeedDesired;
float descentspeedError;
float powerCommand;
float airspeedVector[2];
float fluidMovement[2];
float courseComponent[2];
float courseError;
float courseCommand;
FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
VelocityStateGet(&velocityState);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
AirspeedStateGet(&airspeedState);
SystemSettingsGet(&systemSettings);
/**
* Compute speed error and course
*/
// missing sensors for airspeed-direction we have to assume within
// reasonable error that measured airspeed is actually the airspeed
// component in forward pointing direction
// airspeedVector is normalized
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
// current ground speed projected in forward direction
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
// fluidMovement is a vector describing the aproximate movement vector of
// the surrounding fluid in 2d space (aka wind vector)
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
// calculate the movement vector we need to fly to reach velocityDesired -
// taking fluidMovement into account
courseComponent[0] = velocityDesired.North - fluidMovement[0];
courseComponent[1] = velocityDesired.East - fluidMovement[1];
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
fixedWingSettings->HorizontalVelMin,
fixedWingSettings->HorizontalVelMax);
// if we could fly at arbitrary speeds, we'd just have to move towards the
// courseComponent vector as previously calculated and we'd be fine
// unfortunately however we are bound by min and max air speed limits, so
// we need to recalculate the correct course to meet at least the
// velocityDesired vector direction at our current speed
// this overwrites courseComponent
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
// Error condition: wind speed too high, we can't go where we want anymore
fixedWingPathFollowerStatus.Errors.Wind = 0;
if ((!valid) &&
fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Wind = 1;
result = 0;
}
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = boundf(
velocityDesired.Down,
-fixedWingSettings->VerticalVelMax,
fixedWingSettings->VerticalVelMax);
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast
fixedWingPathFollowerStatus.Errors.Highspeed = 0;
fixedWingPathFollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) {
fixedWingPathFollowerStatus.Errors.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
fixedWingPathFollowerStatus.Errors.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
result = 0;
}
/**
* Compute desired thrust command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
-fixedWingSettings->AirspeedToPowerCrossFeed.Max,
fixedWingSettings->AirspeedToPowerCrossFeed.Max
);
// Compute final thrust response
powerCommand = pid_apply(&PIDpower, -descentspeedError, dT) +
speedErrorToPowerCommandComponent;
// Output internal state to telemetry
fixedWingPathFollowerStatus.Error.Power = descentspeedError;
fixedWingPathFollowerStatus.ErrorInt.Power = PIDpower.iAccumulator;
fixedWingPathFollowerStatus.Command.Power = powerCommand;
// set thrust
stabDesired.Thrust = boundf(fixedWingSettings->ThrustLimit.Neutral + powerCommand,
fixedWingSettings->ThrustLimit.Min,
fixedWingSettings->ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedWingPathFollowerStatus.Errors.Lowpower = 0;
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand >= fixedWingSettings->ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError > 0.0f && // we are too slow already
fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Lowpower = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedWingPathFollowerStatus.Errors.Highpower = 0;
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand <= fixedWingSettings->ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0.0f && // we ARE going up
descentspeedDesired > 0.0f && // we WANT to go down
airspeedError < 0.0f && // we are too fast already
fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Highpower = 1;
result = 0;
}
/**
* Compute desired pitch command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
-fixedWingSettings->VerticalToPitchCrossFeed.Max,
fixedWingSettings->VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
fixedWingPathFollowerStatus.Error.Speed = airspeedError;
fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator;
fixedWingPathFollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand,
fixedWingSettings->PitchLimit.Min,
fixedWingSettings->PitchLimit.Max);
// Error condition: high speed dive
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError < 0.0f && // we are too fast already
fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
result = 0;
}
/**
* Compute desired roll command
*/
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
// overlap calculation. Theres a dead zone behind the craft where the
// counter-yawing of some craft while rolling could render a desired right
// turn into a desired left turn. Making the turn direction based on
// current roll angle keeps the plane committed to a direction once chosen
if (courseError < -180.0f + (fixedWingSettings->ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll > 0.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f - (fixedWingSettings->ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll < 0.0f) {
courseError -= 360.0f;
}
courseCommand = pid_apply(&PIDcourse, courseError, dT);
fixedWingPathFollowerStatus.Error.Course = courseError;
fixedWingPathFollowerStatus.ErrorInt.Course = PIDcourse.iAccumulator;
fixedWingPathFollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = boundf(fixedWingSettings->RollLimit.Neutral +
courseCommand,
fixedWingSettings->RollLimit.Min,
fixedWingSettings->RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
/**
* Compute desired yaw command
*/
// TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0.0f;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
return result;
}
// #include"reaction.h"
int main(){
sky sky;
////////Input Parameters of the potential (fit parameters) /////
std::string parameters_filename="Input.inp";
//change this for neutron or proton
NuclearParameters Nu = read_nucleus_parameters( "Input/nca40.inp" );
double Ef=Nu.Ef;
//lmax can't be 6 for high r value
int lmax = 6;
double z0=20.0;
double zp0;
double A0=40.0;
// +1/2 for proton, -1/2 for neutron
double tz=-0.5;
int type=1;
int mvolume = 4;
int AsyVolume = 1;
double A = 40.0;
if (tz>0) { zp0=1;}
else {zp0=0;}
double ph_gap = Nu.ph_gap;
cout<<"ph_gap = "<<Nu.ph_gap<<endl;
double rStart = .05;
double rmax = 20.;
double ham_pts = 300;
double rdelt = rmax / ham_pts;
// Construct Parameters Object
Parameters p = get_parameters( parameters_filename, Nu.A, Nu.Z, zp0 );
// Construct Potential Object
pot pottt = get_bobs_pot2( type, mvolume, AsyVolume, tz, Nu, p );
pot * pott = &pottt;
// store coulomb potential in order to compare with
boundRspace initiate(rmax , ham_pts , Ef, ph_gap , lmax , Nu.Z , zp0 , Nu.A , pott);
double Elower = -11.61818;
double Eupper = -9.4;
double jj = .5;
int ll = 0;
int Ifine = 1;
initiate.searchNonLoc( Elower, Eupper, jj, ll, Ifine);
initiate.exteriorWaveFunct(ll);
initiate.normalizeWF();
double tol=.01;
double estart=Ef;
///// Making rmesh///
std::vector<double> rmesh_p= initiate.make_rmesh_point();
std::vector<double> rmesh= initiate.make_rmesh();
//std::vector<double> rmesh = initiate.make_rmesh_point();
double rdelt_p = rdelt * 39.0/40.0;
// Create momentum space grid
std::vector<double> kmesh;
std::vector<double> kweights;
double const kmax = 5.0;
int const kpts = rmesh.size();
kmesh.resize( kpts );
kweights.resize( kpts );
GausLeg( 0., kmax, kmesh, kweights );
////////////////////////////
//// s and d wave functions////
///////////////////////////
double J = 0.5;
double Emax=-0.005;
//double Emax = 2.0*Ef;
double Emin=-200.0;
//double Emin=-200.0 + Emax;
std::ofstream filee("waves/wave.out");
std::cout<<Elower<<std::endl;
//Generating the propagator
std::vector< lj_eigen_t > bound_levels = initiate.get_bound_levels( rmesh, tol );
cout<<"energy = "<<bound_levels[1][0].first<<endl;
ofstream boundf("waves/bound.txt");
for(int i=0;i<rmesh.size();i++){
boundf<<rmesh[i]<<" "<<bound_levels[1][0].second[i]*rmesh[i]<<endl;
}
std::vector< mesh_t > emesh_vec =
initiate.get_emeshes( rmesh, Emin, Emax, bound_levels );
cout<<"emesh_vec = "<<emesh_vec.size()<<endl;
std::vector< prop_t > prop_vec = initiate.get_propagators( rmesh, emesh_vec );
vector <double> pdist = initiate.point_distribution(rmesh,Emax,emesh_vec,prop_vec,bound_levels);
double occ[7];
double onum=0.0;
double num=0.0;
double part=0;
double kpart=0;
vector<double> p_dist;
p_dist.assign( rmesh.size(), 0 );
vector<double> k_dist;
k_dist.assign( kmesh.size(), 0 );
matrix_t d_mtx( rmesh.size(), rmesh.size() ); // density matrix
d_mtx.clear();
//Starting loop over L and J to go through all the orbitals
//THESE ARE CHANGED TO PRODUCE A SPECIFIC RESULT, CHANGE THEM BACK FOR GENERAL USE
lmax=3;
for(int L=3;L<lmax+1;L++){
for(int s=0;s<2;s++){
J=L-0.5+s;
if(J<0){
J=0.5;
s=1;
}
cout<<"L = "<<L<<" J = "<<J<<endl;
string jlab = sky.jlab(J);
string llab = sky.llab(L);
string dest = "waves/neutron/occ/";
int index=initiate.index_from_LJ(L,J);
const prop_t &propE = prop_vec.at(index);
const mesh_t &emesh = emesh_vec.at(index);
int Nmax = sky.Nmax(L,J);
double socc[Nmax];
string occlab = dest + "sky" + llab + jlab + ".txt";
ofstream focc(occlab.c_str());
// Create Bessel Function matrix in k and r
/* matrix_t bess_mtx( kmesh.size(), rmesh.size() );
bess_mtx.clear();
matrix_t bess_mtx_sky( kmesh.size(), rmesh.size() );
bess_mtx_sky.clear();
for( unsigned int nk = 0; nk < kmesh.size(); ++nk ) {
for( unsigned int nr = 0; nr < rmesh.size(); ++nr ) {
double rho = kmesh[nk] * rmesh[nr];
bess_mtx( nk, nr ) = gsl_sf_bessel_jl( L, kmesh[nk] * rmesh_p[nr]);
bess_mtx_sky( nk, nr ) = gsl_sf_bessel_jl( L, rho );
}
} */
//Starting loop over N
for(int N=0;N<Nmax;N++){
d_mtx.clear();
cout << "N = "<<N<<endl;
string Nlab;
ostringstream convert;
convert << N;
Nlab = convert.str();
vector<double> wf = sky.read_q(N,L,J,rmesh.size());
vector<double> skyrme = sky.read(N,L,J,rmesh.size());
std::vector<double> spec;
std::vector<double> spec0;
//Folding wave functions with S(r,r';E) to get S(E), does it for dom and skyrme wavefunctions
double spf = 0;
occ[N]=0;
socc[N]=0;
//cout<<"emesh size = "<<emesh.size()<<endl;
for(unsigned int n=0;n<emesh.size();++n){
spf=0.0;
double spf0=0;
double Edelt=emesh[n].second;
for( unsigned int i = 0; i < rmesh.size(); ++i ) {
for( unsigned int j = 0; j < rmesh.size(); ++j ) {
spf -= rdelt * skyrme[i] * skyrme[j] *imag( propE[n]( i, j ) ) / M_PI;
d_mtx(i,j) -= imag(propE[n](i,j)) / M_PI / (rmesh[i] * rmesh[j] * rdelt) * Edelt;
}
}
//WANT TO ADD A COUPLE MORE CATCHES IN HERE FOR VARIOUS OTHER ORBITALS!!!!!!!!!!!!!!!!
if(L==0 && N==1 && emesh[n].first > -9){
spf=0;
}else if(L==2 && N==0 && J==1.5 && emesh[n].first > -10){
spf=0;
}else if(L==3 && N==0 && emesh[n].first > -9){
spf=0;
}
spec.push_back(spf);
spec0.push_back(spf0);
occ[N] += spf0*Edelt;
socc[N] += spf*Edelt;
}
//creating the skyrme density
for(int i=0;i<rmesh.size();i++){
p_dist[i] += socc[N] * (2*J+1) * pow(skyrme[i]/rmesh[i],2) / (4*M_PI);
// k_dist[i] += socc[N] * (2*J+1) * pow(ksky[i],2) / (4*M_PI);
}
//this adds the QP peaks to the particle number
if(L==0 && N==1){
const lj_eigen_t &levels = bound_levels.at(index);
double QPE = levels[N].first;
const std::vector<double> &QPF = levels[N].second;
double S = initiate.sfactor( rmesh, QPE, L, J, QPF );
onum += S*(2*J+1);
occ[N] += S;
for(int i=0;i<rmesh.size();i++){
p_dist[i] += S * (2*J+1) * pow(QPF[i],2) / (4*M_PI);
}
//Transforming QPF to k-space
/* vector <double> kqpf;
kqpf.assign(kmesh.size(),0);
for(int ik=0;ik<kmesh.size();ik++){
double kq=0;
for(int ir=0;ir<rmesh.size();ir++){
kq += rdelt_p * sqrt(2.0/M_PI) * bess_mtx(ik,ir) * QPF[ir] * pow(rmesh_p[ir],2);
}
kqpf[ik] = kq;
}
for(int i=0;i<rmesh.size();i++){
k_dist[i] += S * (2*J+1) * pow(kqpf[i],2) / (4*M_PI);
} */
}else if(L==2 && J==1.5 && N==0){
const lj_eigen_t &levels = bound_levels.at(index);
double QPE = levels[N].first;
const std::vector<double> &QPF = levels[N].second;
double S = initiate.sfactor( rmesh, QPE, L, J, QPF );
onum += S*(2*J+1);
occ[N] += S;
for(int i=0;i<rmesh.size();i++){
p_dist[i] += S * (2*J+1) * pow(QPF[i],2) / (4*M_PI);
}
//see what happens if I find QPE using this self energy
//Transforming QPF to k-space
/* vector <double> kqpf;
kqpf.assign(kmesh.size(),0);
for(int ik=0;ik<kmesh.size();ik++){
double kq=0;
for(int ir=0;ir<rmesh.size();ir++){
kq += rdelt_p * sqrt(2.0/M_PI) * bess_mtx(ik,ir) * QPF[ir] * pow(rmesh_p[ir],2);
}
kqpf[ik] = kq;
}
for(int i=0;i<rmesh.size();i++){
k_dist[i] += S * (2*J+1) * pow(kqpf[i],2) / (4*M_PI);
} */
}
/* if(L ==3 && N == 0){
//double QPEf = initiate.find_level(rmesh, -2.0, N, L, J, tol);
eigen_t forbit = initiate.find_boundstate(rmesh, -2.0, N, L, J, tol);
cout <<"QPE "<<llab<<jlab<<" = "<<forbit.first<<endl;
double fs = initiate.sfactor(rmesh, forbit.first, L, J, forbit.second);
cout <<"S "<<llab<<jlab<<" = "<<fs<<endl;
} */
string speclab = dest + "spec" + llab + jlab + Nlab + ".out";
speclab = "waves/neutron/occ/new/spec" + llab + jlab + Nlab + ".out";
ofstream fspec(speclab.c_str());
for(int i=0;i<emesh.size();i++){
fspec<<emesh[i].first<<" "<<emesh[i].second<<" "<<spec[i]<<endl;
//fspec<<emesh[i].first<<" "<<spec[i]<<endl;
}
//change this to occ if you want dom occupation numbers
focc<<socc[N]<<endl;
onum += socc[N]*(2*J+1);
num += occ[N]*(2*J+1);
} //ending loop over N
} //ending loop over j
} //ending loop over L
ofstream sdenr("waves/skydenr.txt");
ofstream sdenk("waves/skydenk.txt");
for(int i=0;i<rmesh.size();i++){
part += p_dist[i] * pow(rmesh_p[i],2) * rdelt_p * 4 * M_PI;
}
for(int i=0;i<rmesh.size();i++){
sdenr<<rmesh_p[i]<<" "<<p_dist[i]/part<<endl;
}
cout<<"particle number from sky occ = "<<onum<<endl;
cout<<"particle number from sky density (r) = "<<part<<endl;
//out<<"particle number from sky density (k) = "<<kpart<<endl;
ofstream dfile("waves/background/wavedmtxt.txt");
for(int i=0;i<rmesh.size();i++){
for(int j=0;j<rmesh.size();j++){
dfile<<d_mtx(i,j)<<" ";
}
dfile<<endl;
}
}
static inline float CruiseControlLimitThrust(float thrust)
{
// limit to user specified absolute max thrust
return boundf(thrust, stabSettings.cruiseControl.min_thrust, stabSettings.cruiseControl.max_thrust);
}
/**
* WARNING! This callback executes with critical flight control priority every
* time a gyroscope update happens do NOT put any time consuming calculations
* in this loop unless they really have to execute with every gyro update
*/
static void stabilizationInnerloopTask()
{
// watchdog and error handling
{
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
#endif
bool warn = false;
bool error = false;
bool crit = false;
// check if outer loop keeps executing
if (stabSettings.monitor.rateupdates > -64) {
stabSettings.monitor.rateupdates--;
}
if (stabSettings.monitor.rateupdates < -(2 * OUTERLOOP_SKIPCOUNT)) {
// warning if rate loop skipped more than 2 execution
warn = true;
}
if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) {
// critical if rate loop skipped more than 4 executions
crit = true;
}
// check if gyro keeps updating
if (stabSettings.monitor.gyroupdates < 1) {
// error if gyro didn't update at all!
error = true;
}
if (stabSettings.monitor.gyroupdates > 1) {
// warning if we missed a gyro update
warn = true;
}
if (stabSettings.monitor.gyroupdates > 3) {
// critical if we missed 3 gyro updates
crit = true;
}
stabSettings.monitor.gyroupdates = 0;
if (crit) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL);
} else if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
} else if (warn) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
RateDesiredData rateDesired;
ActuatorDesiredData actuator;
StabilizationStatusInnerLoopData enabled;
FlightStatusControlChainData cchain;
RateDesiredGet(&rateDesired);
ActuatorDesiredGet(&actuator);
StabilizationStatusInnerLoopGet(&enabled);
FlightStatusControlChainGet(&cchain);
float *rate = &rateDesired.Roll;
float *actuatorDesiredAxis = &actuator.Roll;
int t;
float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
if (reinit) {
stabSettings.innerPids[t].iAccumulator = 0;
}
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings);
break;
case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING:
rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
);
stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit);
break;
case STABILIZATIONSTATUS_INNERLOOP_AXISLOCK:
if (fabsf(rate[t]) > stabSettings.settings.MaxAxisLockRate) {
// While getting strong commands act like rate mode
axis_lock_accum[t] = 0;
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[t] += (rate[t] - gyro_filtered[t]) * dT;
axis_lock_accum[t] = boundf(axis_lock_accum[t], -stabSettings.settings.MaxAxisLock, stabSettings.settings.MaxAxisLock);
rate[t] = axis_lock_accum[t] * stabSettings.settings.AxisLockKp;
}
// IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication!
// keep order as it is, RATE must follow!
case STABILIZATIONSTATUS_INNERLOOP_RATE:
// limit rate to maximum configured limits (once here instead of 5 times in outer loop)
rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
} else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL:
actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
}
actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
}
actuator.UpdateTime = dT * 1000;
if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
ActuatorDesiredSet(&actuator);
} else {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
{
uint8_t armed;
FlightStatusArmedGet(&armed);
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);
if (armed != FLIGHTSTATUS_ARMED_ARMED ||
((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
}
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
}
