diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h b/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h
index db89e7b1c458f5524a11b023b4352b9b232a4453..3b38f7ebd5e23c0bbb0f40a1d73b6f05e22afd5c 100644
--- a/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h
+++ b/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h
@@ -220,6 +220,8 @@ private:
Sensor<T>& sensor;
PIController pid;
+ uint64_t begin_ts = 0;
+
AirBrakesAlgorithmData algo_data;
AirBrakesData ab_data;
@@ -250,10 +252,15 @@ void AirBrakesControlAlgorithm<T>::begin()
running = true;
+ begin_ts = TimestampTimer::getTimestamp();
+
ts = (sensor.getLastSample()).timestamp;
alpha = computeAlpha(sensor.getLastSample(), true);
+
+#ifndef ROCCARASO
actuator->set(alpha, true);
+#endif
}
template <class T>
@@ -267,11 +274,29 @@ void AirBrakesControlAlgorithm<T>::step()
alpha = computeAlpha(input, false);
}
+#ifndef ROCCARASO
actuator->set(alpha, true);
+#endif
uint64_t t = TimestampTimer::getTimestamp();
logAlgorithmData(input, t);
logAirbrakesData(t);
+
+#ifdef ROCCARASO
+ if (t - begin_ts < 2 * AB_OPENING_TIMEOUT * 1000)
+ { // after 3 seconds open to 100%
+ actuator->set(AB_SERVO_MAX_POS, true);
+ }
+ else if (t - begin_ts > 2 * AB_OPENING_TIMEOUT * 1000 &&
+ t - begin_ts < 3 * AB_OPENING_TIMEOUT * 1000)
+ { // after 6 seconds open to 100%
+ actuator->set(AB_SERVO_MAX_POS / 2, true);
+ }
+ else
+ { // then keep airbrakes closed
+ actuator->set(AB_SERVO_MIN_POS, true);
+ }
+#endif
}
template <class T>
@@ -297,6 +322,11 @@ float AirBrakesControlAlgorithm<T>::computeAlpha(T input, bool firstIteration)
float s = getSurface(z, vz, vMod, rho, u);
float alpha = getAlpha(s);
+ // for logging
+ ab_data.rho = rho;
+ ab_data.u = u;
+ ab_data.surface = s;
+
return alpha;
}
@@ -364,6 +394,7 @@ TrajectoryPoint AirBrakesControlAlgorithm<T>::getSetpoint(float z, float vz)
TrajectoryPoint setpoint = chosenTrajectory.get(indexMinVal);
+ // for logging
ab_data.setpoint_z = setpoint.getZ();
ab_data.setpoint_vz = setpoint.getVz();
@@ -387,7 +418,7 @@ float AirBrakesControlAlgorithm<T>::pidStep(float z, float vz, float vMod,
float u_ref = 0.5 * rho * cd_ref * S0 * vz * vMod;
float error = vz - setpoint.getVz();
- ab_data.pid_error = error;
+ ab_data.pid_error = error; // for logging
// update PI controller
float u = pid.update(error);
@@ -401,38 +432,38 @@ template <class T>
float AirBrakesControlAlgorithm<T>::getSurface(float z, float vz, float vMod,
float rho, float u)
{
- float estimatedCd = 0;
- float selectedS = 0;
- float bestDu = INFINITY;
+ float estimated_cd = 0;
+ float selected_s = 0;
+ float best_du = INFINITY;
for (float s = S_MIN; s < S_MAX + S_STEP; s += S_STEP)
{
float cd = getDrag(vMod, z, s);
float du = abs(u - (0.5 * rho * S0 * cd * vz * vMod));
- if (du < bestDu)
+ if (du < best_du)
{
- bestDu = du;
- selectedS = s;
- estimatedCd = cd;
+ best_du = du;
+ selected_s = s;
+ estimated_cd = cd;
}
}
- ab_data.estimated_cd = estimatedCd;
+ ab_data.estimated_cd = estimated_cd; // for logging
- return selectedS;
+ return selected_s;
}
template <class T>
float AirBrakesControlAlgorithm<T>::getAlpha(float s)
{
- float alphaRadiants =
+ float alpha_rad =
(-B_DELTAS + sqrt(powf(B_DELTAS, 2) + 4 * A_DELTAS * s)) /
(2 * A_DELTAS);
- float alphaDegrees = alphaRadiants * 180.0f / PI;
+ float alpha_deg = alpha_rad * 180.0f / PI;
- return alphaDegrees;
+ return alpha_deg;
}
template <class T>
@@ -461,7 +492,7 @@ float AirBrakesControlAlgorithm<T>::getDrag(float v, float h, float s)
float mach = getMach(v, h);
- float powMach[7] = {1,
+ float pow_mach[7] = {1,
mach,
powf(mach, 2),
powf(mach, 3),
@@ -469,20 +500,20 @@ float AirBrakesControlAlgorithm<T>::getDrag(float v, float h, float s)
powf(mach, 5),
powf(mach, 6)};
- return coeffs.n000 + coeffs.n100 * powMach[1] + coeffs.n200 * powMach[2] +
- coeffs.n300 * powMach[3] + coeffs.n400 * powMach[4] +
- coeffs.n500 * powMach[5] + coeffs.n600 * powMach[6] +
+ return coeffs.n000 + coeffs.n100 * pow_mach[1] + coeffs.n200 * pow_mach[2] +
+ coeffs.n300 * pow_mach[3] + coeffs.n400 * pow_mach[4] +
+ coeffs.n500 * pow_mach[5] + coeffs.n600 * pow_mach[6] +
coeffs.n010 * x + coeffs.n020 * powf(x, 2) +
- coeffs.n110 * x * powMach[1] +
- coeffs.n120 * powf(x, 2) * powMach[1] +
- coeffs.n210 * x * powMach[2] +
- coeffs.n220 * powf(x, 2) * powMach[2] +
- coeffs.n310 * x * powMach[3] +
- coeffs.n320 * powf(x, 2) * powMach[3] +
- coeffs.n410 * x * powMach[4] +
- coeffs.n420 * powf(x, 2) * powMach[4] +
- coeffs.n510 * x * powMach[5] +
- coeffs.n520 * powf(x, 2) * powMach[5] + coeffs.n001 * h;
+ coeffs.n110 * x * pow_mach[1] +
+ coeffs.n120 * powf(x, 2) * pow_mach[1] +
+ coeffs.n210 * x * pow_mach[2] +
+ coeffs.n220 * powf(x, 2) * pow_mach[2] +
+ coeffs.n310 * x * pow_mach[3] +
+ coeffs.n320 * powf(x, 2) * pow_mach[3] +
+ coeffs.n410 * x * pow_mach[4] +
+ coeffs.n420 * powf(x, 2) * pow_mach[4] +
+ coeffs.n510 * x * pow_mach[5] +
+ coeffs.n520 * powf(x, 2) * pow_mach[5] + coeffs.n001 * h;
}
template <class T>
@@ -505,6 +536,7 @@ void AirBrakesControlAlgorithm<T>::logAirbrakesData(uint64_t t)
// pid_error inserted when computing the new alpha (in pidStep())
// setpoint z and vz inserted when computing the new setpoint (in
// getSetpoint())
+ // the same holds to u, surface and rho, in computeAlpha()
logger.log(ab_data);
}
diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesData.h b/src/boards/DeathStack/AirBrakes/AirBrakesData.h
index e3457b340797cd0579567cd92ce6739b3b4a41a0..786fe76e7e7732b43d583eef2bf227cec5471a3e 100644
--- a/src/boards/DeathStack/AirBrakes/AirBrakesData.h
+++ b/src/boards/DeathStack/AirBrakes/AirBrakesData.h
@@ -51,6 +51,9 @@ struct AirBrakesData
{
uint64_t timestamp;
float servo_position;
+ float u;
+ float surface;
+ float rho;
float estimated_cd;
float pid_error;
float setpoint_z;
@@ -58,13 +61,15 @@ struct AirBrakesData
static std::string header()
{
- return "timestamp,servo_position,estimated_cd,pid_error,setpoint_z,setpoint_vz\n";
+ return "timestamp,servo_position,u,surface,rho,estimated_cd,pid_error,"
+ "setpoint_z,setpoint_vz\n";
}
void print(std::ostream& os) const
{
- os << timestamp << "," << servo_position << "," << estimated_cd << ","
- << pid_error << "," << setpoint_z << "," << setpoint_vz << "\n";
+ os << timestamp << "," << servo_position << "," << u << "," << surface
+ << "," << rho << "," << estimated_cd << "," << pid_error << ","
+ << setpoint_z << "," << setpoint_vz << "\n";
}
};
diff --git a/src/boards/DeathStack/configs/AirBrakesConfig.h b/src/boards/DeathStack/configs/AirBrakesConfig.h
index 43a5873564b97c162b2d90072e024a48b52996d6..2e9a5bf3b41021489370252ea664203eb788b4fb 100644
--- a/src/boards/DeathStack/configs/AirBrakesConfig.h
+++ b/src/boards/DeathStack/configs/AirBrakesConfig.h
@@ -71,6 +71,12 @@ static constexpr float AB_SERVO_MAX_RATE = 15.0 / 0.1; // deg/s
static constexpr float AB_SERVO_MIN_RATE = -15.0 / 0.1; // deg/s
static constexpr float AB_SERVO_WIGGLE_AMPLITUDE = 10.0; // deg
+#ifdef ROCCARASO
+// At Roccaraso airbrakes opened at 100%, then 50% and then 0%
+// Each opening is kept for 3 seconds (3000 ms)
+static constexpr float AB_OPENING_TIMEOUT = 3000; // [ms]
+#endif
+
// Control algorithm configs
// static constexpr int LOOKS = 150;
static constexpr int START_INDEX_OFFSET = -1;
@@ -81,12 +87,13 @@ static constexpr float ABK_UPDATE_PERIOD = 0.1 * 1000; // ms -> 10 Hz
#else
static constexpr float ABK_UPDATE_PERIOD = 0.05 * 1000; // ms -> 20 Hz
#endif
+
static constexpr float ABK_UPDATE_PERIOD_SECONDS = ABK_UPDATE_PERIOD / 1000;
#ifdef EUROC
static constexpr int SHADOW_MODE_DURATION = 7.5 * 1000;
#else
-static constexpr int SHADOW_MODE_DURATION = 3.5 * 1000;
+static constexpr int SHADOW_MODE_DURATION = 100; //3.5 * 1000;
#endif
#ifdef EUROC