//==============================================================================
void Button::setToggleState (const bool shouldBeOn,
const bool sendChangeNotification)
{
if (shouldBeOn != lastToggleState)
{
if (getToggleState() != shouldBeOn) // this test means that if the value is void rather than explicitly set to
isOn = shouldBeOn; // false, it won't be changed unless the required value is true.
lastToggleState = shouldBeOn;
repaint();
WeakReference<Component> deletionWatcher (this);
if (sendChangeNotification)
{
sendClickMessage (ModifierKeys());
if (deletionWatcher == nullptr)
return;
}
if (lastToggleState)
{
turnOffOtherButtonsInGroup (sendChangeNotification);
if (deletionWatcher == nullptr)
return;
}
sendStateMessage();
}
}
void Button::setState (const ButtonState newState)
{
if (buttonState != newState)
{
buttonState = newState;
repaint();
if (buttonState == buttonDown)
{
buttonPressTime = Time::getApproximateMillisecondCounter();
lastRepeatTime = 0;
}
sendStateMessage();
}
}
//==============================================================================
void Button::setToggleState (const bool shouldBeOn, const NotificationType notification)
{
if (shouldBeOn != lastToggleState)
{
WeakReference<Component> deletionWatcher (this);
if (shouldBeOn)
{
turnOffOtherButtonsInGroup (notification);
if (deletionWatcher == nullptr)
return;
}
// This test is done so that if the value is void rather than explicitly set to
// false, the value won't be changed unless the required value is true.
if (getToggleState() != shouldBeOn)
{
isOn = shouldBeOn;
if (deletionWatcher == nullptr)
return;
}
lastToggleState = shouldBeOn;
repaint();
if (notification != dontSendNotification)
{
// async callbacks aren't possible here
jassert (notification != sendNotificationAsync);
sendClickMessage (ModifierKeys::getCurrentModifiers());
if (deletionWatcher == nullptr)
return;
}
if (notification != dontSendNotification)
sendStateMessage();
else
buttonStateChanged();
}
}
/* **************************************************************************************
* void stateMachine(struct ETMCVAR* petmcvar);
* @brief : Run state machine
* @param : petmcvar = pointer vars passed from etmc0
* ************************************************************************************** */
void stateMachine(struct ETMCVAR* petmcvar)
{
struct CANRCVBUF can;
if (statevar.launchResetFlag == 1) // init variables for launch
{
// reset variables for next launch
statevar.state = 0;
statevar.tensionMessageFlag = statevar.speedMessageFlag = 0;
statevar.parametersRequestedFlag = 0;
statevar.paramReceivedFlag = 0;
statevar.launchResetFlag = 0;
statevar.filt_torque = 0; // This should be set to 0 on entry to
// Prep from Safe in real system
}
#if LONGTIME
// update the long times
simulationvar.tmpTime = DTWTIME;
// check msbs for overflow toggle
if (((simulationvar.tmpTime & 0x80000000) == 0) && ((simulationvar.oldTime & 0x80000000) == 1))
{ // DTWTIME has overflowed
simulationvar.longTime += 0x0000000100000000;
}
simulationvar.longTime &= 0xffffffff00000000;
simulationvar.longTime |= (u64) simulationvar.tmpTime;
simulationvar.oldTime = simulationvar.tmpTime;
if (((long)(simulationvar.longTime - simulationvar.nextStepTime) >= 0))
//&& (statevar.tensionMessageFlag == 1)
//&& (statevar.speedMessageFlag == 1))
{
(statevar.elapsedTics)++;
can.id = CANID_TIME; // time id
if (++(petmcvar->fracTime) != stateparam.TICSPERSECOND)
{
can.dlc = 1;
can.cd.uc[0] = petmcvar->fracTime;
}
else
{
can.dlc = 5;
can.cd.ui[0] = (petmcvar->unixtime)++;
can.cd.uc[4] = (u8) 0; // status proxy
petmcvar->fracTime = 0;
}
msg_out_mc(&can); // output to CAN+USB
debug_mc_state1 = petmcvar->fracTime;
// next on time Time message time
simulationvar.nextStepTime += stateparam.STEPTIMECLOCKS;
}
#else
if (((int)(DTWTIME - simulationvar.nextStepTime) > 0))
//&& (statevar.tensionMessageFlag == 1)
//&& (statevar.speedMessageFlag == 1))
{
(statevar.elapsedTics)++;
can.id = CANID_TIME; // time id
if (++(petmcvar->fracTime) != stateparam.TICSPERSECOND)
{
can.dlc = 1;
can.cd.uc[0] = petmcvar->fracTime;
}
else
{
can.dlc = 5;
can.cd.ui[0] = (petmcvar->unixtime)++;
can.cd.uc[4] = (u8) 0; // status proxy
petmcvar->fracTime = 0;
}
debug_mc_state1 = petmcvar->fracTime;
msg_out_mc(&can); // output to CAN+USB
debug_mc_state2 = DTWTIME; // Time round trip to PS
// next on time Time message time
simulationvar.nextStepTime += stateparam.STEPTIMECLOCKS;
// dummy control lever messages to flush buffer
can.id = CANID_CONTROL_LEVER;
can.dlc = 0;
can.dlc = 8; // Max size
can.cd.uc[0] = debug_mc_state1; // for debug
for (int i = 0; i < 0; i++)
{
msg_out_mc(&can);
}
}
#endif
// this needs to be moved into SPIInOut()
// convert to a binary word for comparisons (not general for different SPI2SIZE)
cpsw = (((int) petmcvar->spi_swin[0]) << 8) | (int) petmcvar->spi_swin[1];
switch (statevar.state)
{
case 0: // prep
// This will be replaced with detection of pushing the ARM button
// if (calib_control_lever_get() < (float) 0.05)
petmcvar->spi_ledout[1] = 0x01;
if((cpsw & CPARM) == 0)
{
statevar.state = 1; // going to armed state
// setStateled(1); // ??? LED
sendStateMessage(2);
mc_debug_print();
}
break;
case 1: // armed
petmcvar->spi_ledout[1] = 0x01 & petmcvar->ledBlink;
if ((statevar.parametersRequestedFlag == 0)
&& (calib_control_lever_get() > (float) 0.95))
{
// request launch parameters
can.id = CANID_PARAM_REQUEST;
can.cd.uc[0] = debug_mc_state1; // for debug
can.dlc = 0 + 1;
msg_out_mc(&can);
statevar.parametersRequestedFlag = 1;
}
if ((statevar.parametersRequestedFlag == 1)
&& (calib_control_lever_get() < (float) 0.05))
{
// reset and wait for control lever again
statevar.parametersRequestedFlag = 0;
}
// when we get the response, start the simulation
if (statevar.paramReceivedFlag == 1)
{
// simulationvar.startTime = (double) DTWTIME; // not used?
statevar.state = 2;
single_beep();
// setStateled(2); // LED ???
petmcvar->spi_ledout[1] = 0x00;
statevar.startProfileTics = statevar.elapsedTics;
sendStateMessage(3);
mc_debug_print();
}
break;
case 2: // profile 1 soft start
if ((statevar.elapsedTics - statevar.startProfileTics)
>= (stateparam.SOFT_START_TIME * stateparam.TICSPERSECOND))
{
statevar.state = 3;
statevar.peakCableSpeed = measurements.lastCableSpeed;
mc_debug_print();
}
break;
case 3: // profile 2 constant tension ground roll
statevar.peakCableSpeed = measurements.lastCableSpeed > statevar.peakCableSpeed
? measurements.lastCableSpeed : statevar.peakCableSpeed;
if (measurements.lastCableSpeed < (statevar.peakCableSpeed * stateparam.PEAK_CABLE_SPEED_DROP))
{
statevar.state = 4;
statevar.startRampTics = statevar.elapsedTics;
statevar.startRampTension = measurements.lastTension;
single_beep();
sendStateMessage(4);
// setStateled(4);
mc_debug_print();
}
break;
case 4: // ramp
if (statevar.elapsedTics - statevar.startRampTics > stateparam.RAMP_TIME * stateparam.TICSPERSECOND)
{
statevar.state = 5;
// setStateled(5);
single_beep();
sendStateMessage(5);
statevar.minCableSpeed = measurements.lastCableSpeed;
mc_debug_print();
statevar.taperFlag = 0;
}
break;
case 5: // constant
// xprintf(UXPRT,"%6d\n\r", (double) measurements.lastCableSpeed);
if (measurements.lastCableSpeed < statevar.minCableSpeed)
{
statevar.minCableSpeed = measurements.lastCableSpeed;
}
if (measurements.lastCableSpeed > statevar.minCableSpeed + stateparam.RELEASEDELTA)
{
single_beep();
statevar.state = 6;
// setStateled(6);
sendStateMessage(6);
mc_debug_print();
}
break;
case 6: // recovery
//xprintf(UXPRT,"%6d\n\r",measurements.lastCableSpeed);
if (measurements.lastCableSpeed < stateparam.ZERO_CABLE_SPEED_TOLERANCE)
{
statevar.state = 0;
single_beep();
// setStateled(0);
sendStateMessage(1);
mc_debug_print();
statevar.launchResetFlag = 1;
}
break;
} // end of switch (statevar.state)
// Template for Desired Tension and Control Law
if ((statevar.tensionMessageFlag == 1) && (statevar.speedMessageFlag == 1))
{
switch (statevar.state)
{
case 0: // prep
statevar.setptTension = 0;
break;
case 1: // armed
statevar.setptTension = 0;
break;
case 2: // profile 1 soft start
statevar.setptTension = (float) (stateparam.GROUND_TENSION_FACTOR * stateparam.GLIDER_WEIGHT
* 0.5 * (1 - cosf(stateparam.K1 * (statevar.elapsedTics - statevar.startProfileTics))));
break;
case 3: // profile 2 constant tension ground roll with taper
// System.out.println(measurements.lastCableSpeed + stateparam.PROFILE_TRIGGER_CABLE_SPEED);
if (measurements.lastCableSpeed < stateparam.PROFILE_TRIGGER_CABLE_SPEED)
{
statevar.setptTension = stateparam.GROUND_TENSION_FACTOR * stateparam.GLIDER_WEIGHT;
//xprintf(UXPRT,"%6d\n\r", (double) statevar.setptTension); // System.out.println(tension);
}
else
{
statevar.setptTension = (float) (stateparam.GROUND_TENSION_FACTOR * stateparam.GLIDER_WEIGHT * cosf(stateparam.K2 * (measurements.lastCableSpeed - stateparam.PROFILE_TRIGGER_CABLE_SPEED)));
//xprintf(UXPRT,"%6d\n\r", (double) statevar.setptTension); // System.out.println(tension);
}
break;
case 4: // ramp
statevar.setptTension = (float) ((statevar.startRampTension
+ (stateparam.CLIMB_TENSION_FACTOR * stateparam.GLIDER_WEIGHT
- statevar.startRampTension)
* sinf(stateparam.K3 * (statevar.elapsedTics - statevar.startRampTics))));
//xprintf(UXPRT,"%6d\n\r", (double) statevar.setptTension); // System.out.println(tension);
break;
case 5: // constant
statevar.setptTension = (float) (stateparam.CLIMB_TENSION_FACTOR * stateparam.GLIDER_WEIGHT);
if (statevar.taperFlag == 1)
{
statevar.setptTension *= 0.4 + 0.6 * 0.5
* (1 + cosf(stateparam.K4 * (statevar.elapsedTics - statevar.taperTics)));
}
break;
case 6: // recovery
statevar.setptTension = stateparam.MAX_PARACHUTE_TENSION;
if (measurements.lastCableSpeed > stateparam.PROFILE_TRIGGER_CABLE_SPEED)
{
statevar.setptTension *= cosf(stateparam.K5 * (measurements.lastCableSpeed - stateparam.PARACHUTE_TAPER_SPEED));
//xprintf(UXPRT,"%6d\n\r",(double) statevar.setptTension); // System.out.println(tension);
}
break;
}
// scale by control lever
//statevar.setptTension *= calib_control_lever_get(); // comment out to not have to hold handle
// filter the torque with about 1 Hz bandwidth
statevar.setptTorque = statevar.setptTension * stateparam.TENSION_TO_TORQUE;
statevar.filt_torque += (statevar.setptTorque - statevar.filt_torque)
* ((float) 1.0 / ((8 * stateparam.TICSPERSECOND) / 64));
// torqueMessage.set_short((short) (statevar.filt_torque / scaleoffset.torqueScale), 0); // torque
can.id = CANID_TORQUE;
can.dlc = 2 + 1;
can.cd.uc[2] = debug_mc_state1; // for debug
can.cd.us[0] = (short) (statevar.filt_torque / scaleoffset.torqueScale);
msg_out_mc(&can);
statevar.tensionMessageFlag = statevar.speedMessageFlag = 0;
}
}