//-----------------------------------------------------------------------------------------------
// processes the model after the reservoir simulator is run
//-----------------------------------------------------------------------------------------------
void DecoupledModel::process()
{
// update the streams in the pipe network
updateStreams();
// calculating pressures in the Pipe network
calculatePipePressures();
// updating the constraints (this must be done after the pressure calc)
updateConstraints();
// updating the objective
updateObjectiveValue();
// updating the status of the model
setUpToDate(true);
}
void RocketSystem::update() {
//static int time = 0;
//if (time % 1000 == 0) {
// chprintf((BaseSequentialStream*)&SD4, "%d\r\n", RTT2MS(chibios_rt::System::getTime()));
//}
//time = (time+1) % 1000;
// Poll sensors
AccelerometerReading accelReading = accel.readAccel();
GyroscopeReading gyrReading = gyr.readGyro();
optional<AccelerometerReading> accelHReading;
optional<BarometerReading> barReading;
optional<GeigerReading> ggrReading;
optional<GPSReading> gpsReading;
optional<MagnetometerReading> magReading;
if (accelH) accelHReading = (*accelH)->readAccel();
if (bar) barReading = (*bar)->readBar();
if (ggr) ggrReading = (*ggr)->readGeiger();
if (gps) gpsReading = (*gps)->readGPS();
if (mag) magReading = (*mag)->readMag();
SensorMeasurements meas {
.accel = std::experimental::make_optional(accelReading),
.accelH = accelHReading,
.bar = barReading,
.ggr = ggrReading,
.gps = gpsReading,
.gyro = std::experimental::make_optional(gyrReading),
.mag = magReading
};
// Update the world estimate
WorldEstimate estimate = estimator.update(meas);
// Run the controllers
ActuatorSetpoint actuatorSp = {0,0,0,0};
// Run state machine
switch (state) {
case RocketState::DISARMED:
state = DisarmedState(meas, estimate, actuatorSp);
break;
case RocketState::PRE_ARM:
state = PreArmState(meas, estimate, actuatorSp);
break;
case RocketState::ARMED:
state = ArmedState(meas, estimate, actuatorSp);
break;
case RocketState::FLIGHT:
state = FlightState(meas, estimate, actuatorSp);
break;
case RocketState::APOGEE:
state = ApogeeState(meas, estimate, actuatorSp);
break;
case RocketState::DESCENT:
state = DescentState(meas, estimate, actuatorSp);
break;
case RocketState::RECOVERY:
state = RecoveryState(meas, estimate, actuatorSp);
break;
default:
break;
}
// Update motor outputs
motorMapper.run(isArmed(), actuatorSp);
// Poll for controller input
ControllerInput input = inputSource.read();
// Update streams
updateStreams(meas, estimate, actuatorSp);
}
