/// <summary>
/// Predicts forward by the delay time of the tahoe
/// </summary>
/// <param name="absPose"></param>
/// <param name="vehicleState"></param>
public void ForwardPredict(double planningTime, out AbsolutePose absPose, out OperationalVehicleState vehicleState)
{
// get the current state
OperationalVehicleState currentState = Services.StateProvider.GetVehicleState();
// assume the vehicle will hold it current curvature
double curvature = TahoeParams.CalculateCurvature(currentState.steeringAngle, currentState.speed);
// simple dynamics
// xdot = v*cos(theta)
// ydot = v*sin(theta)
// thetadot = v*c
// do euler integration
double dt = 0.01;
double T = TahoeParams.actuation_delay + planningTime;
int n = (int)Math.Round(T/dt);
double v = currentState.speed;
double x = 0, y = 0, heading = 0;
for (int i = 0; i < n; i++) {
x += v*Math.Cos(heading)*dt;
y += v*Math.Sin(heading)*dt;
heading += v*curvature*dt;
}
absPose = new AbsolutePose(new Coordinates(x, y), heading, 0);
vehicleState = currentState;
}
public static void ComputeCommands(SpeedControlData data, OperationalVehicleState vs, out double commandedEngineTorque, out double commandedBrakePressure)
{
Services.Dataset.ItemAs<double>("commanded speed").Add(data.speed.GetValueOrDefault(-1), Services.CarTime.Now);
// speed (m/s)
double v = Math.Abs(vs.speed);
double max_accel;
if (!vs.IsInDrive) {
// we're in reverse
max_accel = 0.65;
}
else if (v < 10*0.44704) {
// below 10 mph
max_accel = 1.0;
}
else if (v < 15*0.44704) {
// below 15 mph
double coeff = (v-10*0.44704)/((15-10)*0.44704);
max_accel = 1.0 - coeff*0.25;
}
else {
max_accel = 0.75;
}
if (data.speed.HasValue && data.speed.Value <= 0 && (!data.accel.HasValue || data.accel.Value <= 0)) {
DateTime now = HighResDateTime.Now;
if (zero_speed_time == DateTime.MinValue) {
zero_speed_time = now;
}
if (now - zero_speed_time < TimeSpan.FromMilliseconds(500) || v < 1) {
commandedEngineTorque = 0;
commandedBrakePressure = TahoeParams.brake_hold;
return;
}
}
else {
zero_speed_time = DateTime.MinValue;
}
// desired speed (m/s)
double rv = Math.Abs(data.speed.GetValueOrDefault(v));
double d_err = 0;
double err = data.speed.HasValue ? v - rv : 0;
if (data.speed.HasValue && !double.IsNaN(prev_err)) {
d_err = (prev_err - err)*Settings.TrackingPeriod;
Services.Dataset.ItemAs<double>("speed - d error").Add(d_err, Services.CarTime.Now);
}
prev_err = err;
// commanded acceleration (m / s^2)
double ra = data.accel.GetValueOrDefault(0) - config.kp*(err) - config.ki*int_err + config.kd*d_err;
// cap the maximum acceleration
if (ra > max_accel)
ra = max_accel;
// add in the zero speed integral term
ra += zero_speed_int*Math.Max(0, 1 - v/zero_speed_int_max_apply_speed);
Services.Dataset.ItemAs<double>("requested acceleration").Add(ra, Services.CarTime.Now);
// compute commands to achieve the requested acceleration and determine if we could achieve those commands
bool couldAchieveRA = SpeedUtilities.GetCommandForAcceleration(ra, vs, out commandedEngineTorque, out commandedBrakePressure);
// only accumulate integral error if we could achieve the target acceleration
if (couldAchieveRA) {
if (ra < max_accel && data.speed.HasValue) {
int_err *= config.ki_leak;
int_err += (err)*Settings.TrackingPeriod;
if (Math.Abs(int_err) > config.ki_cap) {
int_err = Math.Sign(int_err) * config.ki_cap;
}
}
if (v < zero_speed_int_min_speed && ra > 0.2) {
zero_speed_int += zero_speed_int_rate*Settings.TrackingPeriod;
}
}
if (v > zero_speed_int_reset_speed) {
zero_speed_int = 0;
}
Services.Dataset.ItemAs<double>("speed - int error").Add(int_err, Services.CarTime.Now);
}
/// <summary>
/// Computes a commanded torque and brake pressure to bring the vehicle to a stop given the
/// original speed the vehicle was travelling and the remaining distance. This will keep the
/// commanded speed constant until a final approach distance and then linearly ramp down the
/// speed.
/// </summary>
/// <param name="remaining">Remaining distance to stop in meters</param>
/// <param name="origSpeed">Approach speed of stop maneuver in m/s</param>
/// <param name="inFinalApproach">Flag indicating that the final approach is in progress. Only set to true.</param>
/// <returns>Engine torque and brake pressure to be commanded to stop as desired</returns>
public static bool GetStoppingSpeedCommand(double remaining, double origSpeed, OperationalVehicleState vs, ref double commandedSpeed)
{
// check if we're within the stopping tolerance
if ((remaining <= stop_dist_tolerance && Math.Abs(vs.speed) <= stop_speed_tolerance) || remaining <= 0) {
// return a commanded speed of 0 which flags that we should just stop
commandedSpeed = 0;
// return that we've stoppped
return Math.Abs(vs.speed) <= stop_speed_tolerance;
}
// get the distance for the final approach
double stoppingDist = 4;
// check if the remaining distance is greater than the stopping distance or not
if (remaining > stoppingDist) {
// we have not reached the final approach yet, so keep commanding the original speed
commandedSpeed = Math.Max(origSpeed, min_cmd_speed);
}
else {
// we are within the final approach distance so linearly ramp down the speed
commandedSpeed = GetPowerProfileVelocity(origSpeed, 0, stoppingDist, remaining);
}
return false;
}
/// <summary>
/// Computes the engine torque and brake pressure needed to achieve a specified acceleration.
/// </summary>
/// <param name="ra">Requested acceleration in m/s^2</param>
/// <param name="vs">Vehicle state</param>
/// <param name="commandedEngineTorque"></param>
/// <param name="commandedBrakePressure"></param>
/// <returns>True if the vehicle state allows the acceleration to be achieved, false otherwise.</returns>
public static bool GetCommandForAcceleration(double ra, OperationalVehicleState vs, out double commandedEngineTorque, out double commandedBrakePressure)
{
// speed, positive for forward motion (m/s)
double v = Math.Abs(vs.speed);
// current gear (not just shifter but also first, second, etc)
TransmissionGear gear = vs.transGear;
// vehicle pitch, from pose (rad)
double phi = vs.pitch;
if (gear == TransmissionGear.Reverse) {
phi = -phi;
}
CarTimestamp lastTransChangeTimeTS = Services.Dataset.ItemAs<DateTime>("trans gear change time").CurrentTime;
DateTime lastTransChangeTime = Services.Dataset.ItemAs<DateTime>("trans gear change time").CurrentValue;
if (gear > TransmissionGear.Fourth || gear < TransmissionGear.Reverse || (lastTransChangeTimeTS.IsValid && (HighResDateTime.Now - lastTransChangeTime) < TimeSpan.FromSeconds(1) && (gear == TransmissionGear.First || gear == TransmissionGear.Reverse))) {
// in park or neutral or have not waited sufficient time after shift, just break out of the function
commandedEngineTorque = 0;
commandedBrakePressure = -TahoeParams.bc0;
return false;
}
// current gear ratio
double Nt = Math.Abs(TahoeParams.Nt[(int)gear]);
// initialize to reasonable default values in case all else fails
commandedEngineTorque = 0;
commandedBrakePressure = -TahoeParams.bc0;
// effective rotating mass
double mr = TahoeParams.mr1 +
TahoeParams.mr2 * Math.Pow(TahoeParams.Nf, 2) +
TahoeParams.mr3 * Math.Pow(TahoeParams.Nf, 2) * Math.Pow(Nt, 2);
// rolling resistance (N)
double Rr = 2*TahoeParams.m * TahoeParams.g * (TahoeParams.rr1 + TahoeParams.rr2 * v);
double rps2rpm = 60.0 / (2.0 * Math.PI);
// calculate engine speed in rpm
double rpme = vs.engineRPM;
// calculate driveshaft rpm post torque converter
double rpmd = (v / TahoeParams.r_wheel) * TahoeParams.Nf * Nt * rps2rpm;
// calculate speed ratio, torque ratio, and capacity factor from lookups
double sr = rpmd / rpme;
// tr = interp1(srlu, trlu, sr, 'linear', 'extrap');
double tr = TahoeParams.TorqueRatio(sr);
//Dataset.Source.DatasetSource ds = Operational.Instance.Dataset;
// calculate bleed-off torque
double bleed_torque = TahoeParams.bleed_off_power / (rpme / rps2rpm);
// requested acceleration in Newtons
double ra_corr = (TahoeParams.m + mr) * ra;
// drag force
double drag = 0.5 * TahoeParams.rho * TahoeParams.cD * TahoeParams.cxa * Math.Pow(v, 2);
// pitch force
double pitch_corr = -TahoeParams.m * TahoeParams.g * Math.Sin(phi);
// needed wheel torque (N-m)
double wheelTorque = (ra_corr + Rr + drag + pitch_corr) * TahoeParams.r_wheel;
// commanded engine torque (N-m) to achieve desired acceleration
commandedEngineTorque = wheelTorque / (Nt * TahoeParams.Nf * TahoeParams.eta * tr);
DateTime now = DateTime.Now;
//ds.ItemAs<double>("torque multiplier").Add(tr, now);
//ds.ItemAs<double>("speed - Rr").Add(Rr, now);
//ds.ItemAs<double>("speed - ra").Add(ra_corr, now);
//ds.ItemAs<double>("speed - pitch").Add(pitch_corr, now);
//ds.ItemAs<double>("speed - drag").Add(drag, now);
//ds.ItemAs<double>("speed - wheel torque").Add(wheelTorque, now);
//ds.ItemAs<double>("speed - eng torque").Add(commandedEngineTorque, now);
// retrieve the current engine torque, brake pressure
double current_brake_pressure = vs.brakePressure;
// decide to apply master cylinder pressure or engine torque
// check if we want to apply less torque than is the minimum
if (commandedEngineTorque < TahoeParams.Te_min) {
// assume that the current engine torque is just the minimum delivered engine torque
double current_engine_torque = TahoeParams.Te_min;
double Btrq = Nt * TahoeParams.Nf * TahoeParams.eta / TahoeParams.r_wheel;
// actual acceleration with no extra torque applied (m / s^2)
double aa = -(Rr + 0.5 * TahoeParams.rho * TahoeParams.cD * TahoeParams.cxa * Math.Pow(v, 2) -
TahoeParams.m * TahoeParams.g * Math.Sin(phi)) / (TahoeParams.m + mr) +
(Btrq*tr*(current_engine_torque-bleed_torque)) / (TahoeParams.m + mr);
// residual brake acceleration (m / s^2)
double ba = ra - aa; // if ba > 0, engine braking is enough
// desired master cylinder pressure (GM units, approx 22 to 50)
double mcp = -TahoeParams.bc0;
if (ba < 0) {
// compute braking coefficent (which was empirically determined to be speed dependent)
double bc = TahoeParams.bc1 + TahoeParams.bcs*Math.Sqrt(v);
mcp = (-(TahoeParams.m + mr) * ba * TahoeParams.r_wheel / bc) - TahoeParams.bc0;
}
// set commanded engine torque to 0
commandedEngineTorque = 0;
// assign master cylinder pressure
commandedBrakePressure = Math.Round(mcp, 0);
return true;
}
else {
// we want to command engine torque
// check if the brake is off (if not, don't apply engine torque)
if (current_brake_pressure >= 24) {
commandedBrakePressure = -TahoeParams.bc0;
commandedEngineTorque = 0;
return false;
}
// add in the bleed off torque
commandedEngineTorque += bleed_torque;
// zero out the commande brake pressure
commandedBrakePressure = -TahoeParams.bc0;
return true;
}
}
protected bool BeginProcess()
{
if (cancelled)
return false;
curTimestamp = Services.RelativePose.CurrentTimestamp;
vs = Services.StateProvider.GetVehicleState();
// check if we're currently in drive
bool transBypassed = Services.Dataset.ItemAs<bool>("trans bypassed").CurrentValue;
if (!Settings.TestMode && !transBypassed && ((!reverseGear && !vs.IsInDrive) || (reverseGear && vs.transGear != TransmissionGear.Reverse))) {
BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "shifting to drive");
// execute a shift command
Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetShiftTransmissionCommand(reverseGear ? TransmissionGear.Reverse : TransmissionGear.First, null, null, false));
// don't process anything else
return false;
}
InitializePlanningSettings();
// return true to indicate that processing should continue
return true;
}
