void calib_control_lever(struct ETMCVAR* petmcvar)
{
int clcalstate = 0; // state for control lever intial calibration
int i; // loop counter
int adc_tmp;
unsigned int t_led = DTWTIME + FLASHCOUNT; // initial t_led
char vv[128];
int ledCount = 0;
int cycleCount = 1;
#define ledPatternLength 32
#define ledLag 3
// led test pattern with extension to effect circular behavior
u32 ledTestPattern[] =
{
0xffff,
0xffff,
0x0000,
0x0000,
0x0000,
0x0000,
0xffff,
0xffff,
0x0000,
0x0000,
0x0000,
0x0000,
LED_SAFE,
LED_PREP,
LED_ARM,
LED_STOP,
LED_GNDRLRTN,
LED_RAMP,
LED_CLIMB,
LED_ABORT,
LED_RECOVERY,
LED_PREP,
LED_RETRIEVE,
LED_SAFE,
LED_ARM_PB,
LED_PREP_PB,
0x0000,
0x0000,
0x0000,
0x0000,
0x0000,
0x0000,
0xffff,
0xffff,
0x0000,
0x0000,
0x0000,
0x0000
};
if (GPIOB_IDR & (1 << 1)) // test for physical or glass CP
{ // physical CP
xprintf (UXPRT,"\nBegin control lever calibration\n\r");
// dummy read of SPI switches to deal with false 0000 initially returned
spi2_rw(petmcvar->spi_ledout, petmcvar->spi_swin, SPI2SIZE);
while(clcalstate < 6)
{
if (((int)(DTWTIME - t_led)) > 0) // Has the time expired?
{ // Time expired
// read filtered control lever adc last value and update min and max values
adc_tmp = adc_last_filtered[CL_ADC_CHANNEL];
cloffset = (cloffset < adc_tmp) ? cloffset : adc_tmp;
clmax = (clmax > adc_tmp) ? clmax : adc_tmp;
// Read SPI switches
// get most current switch positions
// Not sure why
/*if (spi2_busy() != 0) // Is SPI2 busy?
{ // SPI completed
spi2_rw(petmcvar->spi_ledout, petmcvar->spi_swin, SPI2SIZE); // Send/rcv SPI2SIZE bytes
// convert to a binary word for comparisons (not general for different SPI2SIZE)
sw = (((int) petmcvar->spi_swin[0]) << 8) | (int) petmcvar->spi_swin[1];
//sw = petmcvar->spi_swin; */
// usage of spi2rw() is bad. Should wait for not busy after starting it.
if (spi2_busy() != 0) // Is SPI2 busy?
{ // Here, no.
u32 tmp = petmcvar->cp_outputs;
for (i = SPI2SIZE - 1; i >= 0; i--)
{
petmcvar->spi_ledout[i] = (char) tmp;
tmp >>= 8;
}
petmcvar->cp_inputs = (((int) petmcvar->spi_swin[0]) << 8) | (int) petmcvar->spi_swin[1];
spi2_rw(petmcvar->spi_ledout, petmcvar->spi_swin, SPI2SIZE);
xprintf(UXPRT, "%5u %5d %8x \n\r", clcalstate, adc_tmp, petmcvar->cp_inputs);
petmcvar->cp_outputs = 0;
if ((clcalstate == 1) || (clcalstate == 2))
{
// LEDs chasing their tails
for (i = 0; i < ledLag; i++)
{
petmcvar->cp_outputs |= ledTestPattern[ledCount + i];
}
}
switch(clcalstate)
{
case 0: // entry state
{
sprintf(vv, "Cycle control lever");
lcd_printToLine(UARTLCD, 0, vv);
sprintf(vv, "twice:");
lcd_printToLine(UARTLCD, 1, vv);
double_beep();
clcalstate = 1;
}
case 1: // waiting for CL to rest position
{
if (petmcvar->cp_inputs & CLREST) break;
clcalstate = 2;
cloffset = clmax = adc_tmp; // reset min and max values
sprintf(vv, "twice: 0");
lcd_printToLine(UARTLCD, 1, vv);
break;
}
case 2: // waiting for full scale position first time
{
if (petmcvar->cp_inputs & CLFS) break ;
clcalstate = 3;
sprintf(vv, "twice: 0.5");
lcd_printToLine(UARTLCD, 1, vv);
break;
}
case 3: // wating for return to rest first time
{
if (petmcvar->cp_inputs & CLREST) break;
clcalstate = 4;
// clcalstate = 6; // only requires 1 cycle
sprintf(vv, "twice: 1 ");
lcd_printToLine(UARTLCD, 1, vv);
single_beep();
break;
}
case 4: // waiting for full scale second time
{
if (petmcvar->cp_inputs & CLFS) break;
clcalstate = 5;
sprintf(vv, "twice: 1.5");
lcd_printToLine(UARTLCD, 1, vv);
break;
}
case 5: // waiting for return to rest second time
{
if (petmcvar->cp_inputs & CLREST) break;
single_beep();
clcalstate = 6;
}
}
}
toggle_4leds(); // Advance some LED pattern
ledCount++;
if (ledCount >= ledPatternLength)
{
ledCount = 0;
}
t_led += FLASHCOUNT; // Set next toggle time
}
/* **************************************************************************************
* 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;
}
}