void pyro_fire(uint8_t channel, uint16_t duration_ms)
{
uint8_t pad;
if(channel == 1) {
pad = GPIOE_PY1_TRG;
chVTReset(&vt1);
chVTSet(&vt1, MS2ST(duration_ms), pyro_off_1, NULL);
log_s16(CHAN_PYRO_F, 1, 0, 0, 0);
} else if(channel == 2) {
pad = GPIOE_PY2_TRG;
chVTReset(&vt2);
chVTSet(&vt2, MS2ST(duration_ms), pyro_off_2, NULL);
log_s16(CHAN_PYRO_F, 0, 1, 0, 0);
} else if(channel == 3) {
pad = GPIOE_PY3_TRG;
chVTReset(&vt3);
chVTSet(&vt3, MS2ST(duration_ms), pyro_off_3, NULL);
log_s16(CHAN_PYRO_F, 0, 0, 1, 0);
} else if(channel == 4) {
pad = GPIOE_PY4_TRG;
chVTReset(&vt4);
chVTSet(&vt4, MS2ST(duration_ms), pyro_off_4, NULL);
log_s16(CHAN_PYRO_F, 0, 0, 0, 1);
} else {
return;
}
palSetPad(GPIOE, pad);
}
/**
* @brief Starts the interrupts and timers so sensors will be
* serviced.
*
* @param[in] srdp Pointer to the SensorReadDriver object.
*
* @return The operation status.
* @retval MSG_OK The enabling of interrupts and starting of the
* virtual timers was successful.
* @retval MSG_RESET The driver was not in the correct state.
*
* @api
*/
msg_t SensorReadStart(SensorReadDriver *srdp)
{
size_t i;
chDbgCheck(srdp != NULL);
if (srdp->state == SRD_STOPPED)
{
/* Enable interrupts for interrupt driven sensors */
for (i = 0; i < srdp->interrupt_sensor_cnt; i++)
extChannelEnable(&SRD_EXT_DRIVER,
srdp->interrupt_sensor_ptr[i].interrupt_channel);
/* Enable timers for polled driven sensors */
for (i = 0; i < srdp->polled_sensor_cnt; i++)
/* The parameter sent to the VT is the pointer to the corresponding
polled_sensor_t structure to be able to access it from the
callback function */
chVTSet(srdp->polled_sensor_ptr[i].polling_vt,
CH_CFG_ST_FREQUENCY / srdp->polled_sensor_ptr[i].frequency_hz,
sensors_polled_callback,
(void *)&srdp->polled_sensor_ptr[i]);
/* Everything OK, transverse the state */
srdp->state = SRD_STARTED;
return MSG_OK;
}
else
return MSG_RESET;
}
static THD_FUNCTION(spi_thread, p) {
unsigned i;
SPIDriver *spip = (SPIDriver *)p;
virtual_timer_t vt;
uint8_t txbuf[256];
uint8_t rxbuf[256];
chRegSetThreadName("SPI overlord");
chVTObjectInit(&vt);
/* Prepare transmit pattern.*/
for (i = 0; i < sizeof(txbuf); i++)
txbuf[i] = (uint8_t)i;
/* Continuous transmission.*/
while (true) {
/* Starts a VT working as watchdog to catch a malfunction in the SPI
driver.*/
chVTSet(&vt, MS2ST(10), tmo, (void *)"SPI timeout");
spiExchange(spip, sizeof(txbuf), txbuf, rxbuf);
/* Stops the watchdog.*/
chVTReset(&vt);
}
}
void LedSmooth_t::SetSmoothly(uint16_t AValue) {
IState = slsNone;
if(INeededValue == AValue) return;
chVTReset(&ITmr);
INeededValue = AValue;
uint32_t Delay = ISetupDelay(ICurrentValue);
chVTSet(&ITmr, MS2ST(Delay), LedTmrCallback, NULL);
}
/**
* @brief Start a timer.
*/
osStatus osTimerStart(osTimerId timer_id, uint32_t millisec) {
if (millisec == 0)
return osErrorValue;
timer_id->millisec = millisec;
chVTSet(&timer_id->vt, millisec, (vtfunc_t)timer_cb, timer_id);
return osOK;
}
/**
* @brief Start a timer.
*/
osStatus osTimerStart(osTimerId timer_id, uint32_t millisec) {
if ((millisec == 0) || (millisec == osWaitForever))
return osErrorValue;
timer_id->millisec = millisec;
chVTSet(&timer_id->vt, MS2ST(millisec), (vtfunc_t)timer_cb, timer_id);
return osOK;
}
void LedSmooth_t::Glimmer(uint16_t AMax, uint16_t AMin) {
chVTReset(&ITmr);
IState = slsGlimmer;
IMax = AMax;
IMin = AMin;
if(ICurrentValue < IMax) INeededValue = IMax;
else INeededValue = IMin;
uint32_t Delay = ISetupDelay(ICurrentValue);
chVTSet(&ITmr, MS2ST(Delay), LedTmrCallback, NULL);
}
static void checkFunc(void * param) {
if (!pilotGetPower()) {
on = true;
palSetPad(POWER_PORT, POWER_PAD);
} else {
on = false;
palClearPad(POWER_PORT, POWER_PAD);
}
chVTSet(&vt, MS2ST(POLL_INTERVAL), checkFunc, NULL);
}
/**
* @brief Starts the timer
* @details If the timer was already running then the function has no effect.
*
* @param[in] etp pointer to an initialized @p event_timer_t structure.
*/
void evtStart(event_timer_t *etp) {
chVTSet(&etp->et_vt, etp->et_interval, tmrcb, etp);
}
void powerSetup() {
pilotSetup();
palSetPadMode(POWER_PORT, POWER_PAD, PAL_MODE_OUTPUT_PUSHPULL);
palClearPad(POWER_PORT, POWER_PAD);
chVTSet(&vt, MS2ST(POLL_INTERVAL), checkFunc, NULL);
}
CCM_FUNC static THD_FUNCTION(ThreadADC, arg)
{
(void)arg;
chRegSetThreadName("Sensors");
adcsample_t * sensorsDataPtr;
size_t n;
uint16_t i, pos;
uint32_t an[3] = {0, 0, 0};
median_t an1, an2, an3;
uint8_t row, col;
chThdSleepMilliseconds(250);
timcapEnable(&TIMCAPD3);
chVTSet(&vt_freqin, FREQIN_INTERVAL, freqinVTHandler, NULL);
adcStartConversion(&ADCD1, &adcgrpcfg_sensors, samples_sensors, ADC_GRP1_BUF_DEPTH);
median_init(&an1, 0 , an1_buffer, ADC_GRP1_BUF_DEPTH/2);
median_init(&an2, 0 , an2_buffer, ADC_GRP1_BUF_DEPTH/2);
median_init(&an3, 0 , an3_buffer, ADC_GRP1_BUF_DEPTH/2);
while (TRUE)
{
while (!recvFreeSamples(&sensorsMb, (void*)&sensorsDataPtr, &n))
chThdSleepMilliseconds(5);
an[0]= 0;
an[1]= 0;
an[2]= 0;
/* Filtering and adding */
for (i = 0; i < (n/ADC_GRP1_NUM_CHANNELS); i++)
{
pos = i * ADC_GRP1_NUM_CHANNELS;
an[0] += median_filter(&an1, sensorsDataPtr[pos]);
an[1] += median_filter(&an2, sensorsDataPtr[pos+1]);
an[2] += median_filter(&an3, sensorsDataPtr[pos+2]);
}
/* Averaging */
an[0] /= (n/ADC_GRP1_NUM_CHANNELS);
an[1] /= (n/ADC_GRP1_NUM_CHANNELS);
an[2] /= (n/ADC_GRP1_NUM_CHANNELS);
/* Convert to milliVolts */
an[0] *= VBAT_RATIO;
an[1] *= AN_RATIO;
an[2] *= AN_RATIO;
sensors_data.an1 = an[0];
sensors_data.an2 = an[1];
sensors_data.an3 = an[2];
/* Analog/Digital Sensors */
if (settings.sensorsInput == SENSORS_INPUT_DIRECT) {
sensors_data.tps = calculateTpFromMillivolt(settings.tpsMinV, settings.tpsMaxV, sensors_data.an2);
sensors_data.rpm = calculateFreqWithRatio(sensors_data.freq1, settings.rpmMult);
sensors_data.spd = calculateFreqWithRatio(sensors_data.freq2, settings.spdMult);
}
else if (settings.sensorsInput == SENSORS_INPUT_TEST) {
sensors_data.tps = rand16(0, 200);
sensors_data.rpm = rand16(10, 18000);
sensors_data.spd = rand16(5, 10000);
}
/* AFR */
if (settings.afrInput == AFR_INPUT_AN) {
sensors_data.afr = calculateAFRFromMillivolt(settings.AfrMinVal, settings.AfrMaxVal, sensors_data.an3);
}
else if (settings.afrInput == AFR_INPUT_TEST) {
sensors_data.afr = rand16(11000, 16000) / 100;
}
if (dbg_sensors) {
chprintf(DBG_STREAM,"->[SENSORS] TPS mV/PCT: %06u/%04u\r\n", sensors_data.an2, sensors_data.tps);
chprintf(DBG_STREAM,"->[SENSORS] RMP Hz/Mult/Val: %06u/%.4f/%04u\r\n", sensors_data.freq1, settings.rpmMult, sensors_data.rpm);
chprintf(DBG_STREAM,"->[SENSORS] SPD Hz/Mult/Val: %06u/%.4f/%04u\r\n", sensors_data.freq2, settings.spdMult, sensors_data.spd);
}
if (findCell(sensors_data.tps/2, sensors_data.rpm, &row, &col))
{
sensors_data.cell.row = row;
sensors_data.cell.col = col;
if (dbg_sensors) {
chprintf(DBG_STREAM,"->[SENSORS] Row:Value/Col:Value: %02u:%05u/%02u:%05u\r\n", row, tableRows[row], col, tableColumns[col]*100);
}
if ((settings.functions & FUNC_RECORD) && sensors_data.rpm != 0)
{
/* Average */
tableAFR[row][col] = tableAFR[row][col] == 0 ? sensors_data.afr : ((uint16_t)sensors_data.afr+(uint16_t)tableAFR[row][col])/2;
/* Peaks */
tableKnock[row][col] = sensors_data.knock_value > tableKnock[row][col] ? sensors_data.knock_value : tableKnock[row][col];
}
}
}
return;
}
/*
* Application entry point.
*/
int main(void) {
unsigned i;
static uint8_t patterns1[4096], patterns2[4096], buf1[4096], buf2[4096];
/* System initializations.
- HAL initialization, this also initializes the configured device drivers
and performs the board-specific initializations.
- Kernel initialization, the main() function becomes a thread and the
RTOS is active.*/
halInit();
chSysInit();
/* Creates the blinker thread.*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO + 10,
Thread1, NULL);
/* Activates the ADC1 driver and the temperature sensor.*/
adcStart(&ADCD1, NULL);
adcSTM32EnableTSVREFE();
/* Starts an ADC continuous conversion and its watchdog virtual timer.*/
chVTSet(&adcvt, MS2ST(10), tmo, (void *)"ADC timeout");
adcStartConversion(&ADCD1, &adcgrpcfg2, samples2, ADC_GRP2_BUF_DEPTH);
/* Activating SPI drivers.*/
spiStart(&SPID1, &hs_spicfg);
spiStart(&SPID2, &hs_spicfg);
spiStart(&SPID3, &hs_spicfg);
/* Starting SPI threads instances.*/
chThdCreateStatic(waSPI1, sizeof(waSPI1), NORMALPRIO + 1, spi_thread, &SPID1);
chThdCreateStatic(waSPI2, sizeof(waSPI2), NORMALPRIO + 1, spi_thread, &SPID2);
chThdCreateStatic(waSPI3, sizeof(waSPI3), NORMALPRIO + 1, spi_thread, &SPID3);
/* Allocating two DMA2 streams for memory copy operations.*/
if (dmaStreamAllocate(STM32_DMA2_STREAM6, 0, NULL, NULL))
chSysHalt("DMA already in use");
if (dmaStreamAllocate(STM32_DMA2_STREAM7, 0, NULL, NULL))
chSysHalt("DMA already in use");
for (i = 0; i < sizeof (patterns1); i++)
patterns1[i] = (uint8_t)i;
for (i = 0; i < sizeof (patterns2); i++)
patterns2[i] = (uint8_t)(i ^ 0xAA);
/* Normal main() thread activity, it does continues memory copy operations
using 2 DMA streams at the lowest priority.*/
while (true) {
virtual_timer_t vt;
chVTObjectInit(&vt);
/* Starts a VT working as watchdog to catch a malfunction in the DMA
driver.*/
chVTSet(&vt, MS2ST(10), tmo, (void *)"copy timeout");
/* Copy pattern 1.*/
dmaStartMemCopy(STM32_DMA2_STREAM6,
STM32_DMA_CR_PL(0) | STM32_DMA_CR_PSIZE_BYTE |
STM32_DMA_CR_MSIZE_BYTE,
patterns1, buf1, sizeof (patterns1));
dmaStartMemCopy(STM32_DMA2_STREAM7,
STM32_DMA_CR_PL(0) | STM32_DMA_CR_PSIZE_BYTE |
STM32_DMA_CR_MSIZE_BYTE,
patterns1, buf2, sizeof (patterns1));
dmaWaitCompletion(STM32_DMA2_STREAM6);
dmaWaitCompletion(STM32_DMA2_STREAM7);
if (memcmp(patterns1, buf1, sizeof (patterns1)))
chSysHalt("pattern error");
if (memcmp(patterns1, buf2, sizeof (patterns1)))
chSysHalt("pattern error");
/* Copy pattern 2.*/
dmaStartMemCopy(STM32_DMA2_STREAM6,
STM32_DMA_CR_PL(0) | STM32_DMA_CR_PSIZE_BYTE |
STM32_DMA_CR_MSIZE_BYTE,
patterns2, buf1, sizeof (patterns2));
dmaStartMemCopy(STM32_DMA2_STREAM7,
STM32_DMA_CR_PL(0) | STM32_DMA_CR_PSIZE_BYTE |
STM32_DMA_CR_MSIZE_BYTE,
patterns2, buf2, sizeof (patterns2));
dmaWaitCompletion(STM32_DMA2_STREAM6);
dmaWaitCompletion(STM32_DMA2_STREAM7);
if (memcmp(patterns2, buf1, sizeof (patterns2)))
chSysHalt("pattern error");
if (memcmp(patterns2, buf2, sizeof (patterns2)))
chSysHalt("pattern error");
/* Stops the watchdog.*/
chVTReset(&vt);
chThdSleepMilliseconds(2);
}
return 0;
}
void motorRightBackwardS(int milli){
motor_right_on(D_BACKWARD);
chVTSet(&vtr, MS2ST(milli), motor_right_off, NULL);
}
void motorRightForwardS(int milli){
motor_right_on(D_FORWARD);
chVTSet(&vtr, MS2ST(milli), motor_right_off, NULL);
}
void motorLeftBackwardS(int milli){
motor_left_on(D_BACKWARD);
chVTSet(&vtl, MS2ST(milli), motor_left_off, NULL);
}
//enable motors for specific time
void motorLeftForwardS(int milli){
motor_left_on(D_FORWARD);
chVTSet(&vtl, MS2ST(milli), motor_left_off, NULL);
}
/**
* @brief Starts the test timer.
*
* @param[in] ms time in milliseconds
*/
void test_start_timer(unsigned ms) {
systime_t duration = MS2ST(ms);
test_timer_done = FALSE;
chVTSet(&vt, duration, tmr, NULL);
}
void Pin_t::Pulse(uint32_t ms) {
chVTReset(&ITmr);
High();
// Start timer to switch off
chVTSet(&ITmr, MS2ST(ms), PinTmrCallback, NULL);
}
