/**
* @brief Adds a set of event flags directly to specified @p Thread.
*
* @param[in] tp the thread to be signaled
* @param[in] mask the event flags set to be ORed
*
* @api
*/
void chEvtSignal(Thread *tp, eventmask_t mask) {
chDbgCheck(tp != NULL, "chEvtSignal");
chSysLock();
chEvtSignalI(tp, mask);
chSchRescheduleS();
chSysUnlock();
}
/**
* @brief Adds a set of event flags directly to the specified @p thread_t.
*
* @param[in] tp the thread to be signaled
* @param[in] events the events set to be ORed
*
* @api
*/
void chEvtSignal(thread_t *tp, eventmask_t events) {
chDbgCheck(tp != NULL);
chSysLock();
chEvtSignalI(tp, events);
chSchRescheduleS();
chSysUnlock();
}
void Keys_t::AddEvtToQueue(KeyEvtInfo_t Evt) {
// Uart.Printf("EvtType=%u; Keys: ", Evt.Type);
// for(uint8_t i=0; i<Evt.NKeys; i++) Uart.Printf("%u ", Evt.KeyID[i]);
// Uart.Printf("\r\n");
if(App.PThd == nullptr) return;
chSysLock();
EvtBuf.Put(&Evt);
chEvtSignalI(App.PThd, EVTMSK_KEYS);
chSysUnlock();
}
/**
* @brief Send signals.
*/
int32_t osSignalSet(osThreadId thread_id, int32_t signals) {
int32_t oldsignals;
syssts_t sts = chSysGetStatusAndLockX();
oldsignals = (int32_t)thread_id->p_epending;
chEvtSignalI((thread_t *)thread_id, (eventmask_t)signals);
chSysRestoreStatusX(sts);
return oldsignals;
}
void Keys_t::AddEvtToQueue(KeyEvt_t AType, uint8_t KeyIndx) {
KeyEvtInfo_t IEvt;
IEvt.Type = AType;
IEvt.KeysCnt = 1;
IEvt.KeyID[0] = KeyIndx;
chSysLock();
EvtBuf.Put(&IEvt);
chEvtSignalI(App.PThd, EVTMSK_KEYS);
chSysUnlock();
}
void AdcTxIrq(void *p, uint32_t flags) {
dmaStreamDisable(ADC_DMA);
ADC1->CR2 = 0; // Disable ADC
// Signal event to thread
if(IPThread != nullptr) {
chSysLockFromIsr();
chEvtSignalI(IPThread, IEvt);
chSysUnlockFromIsr();
}
}
/*
* Called every time new ADC values are available. Note that
* the ADC is initialized from mcpwm.c
*/
void main_dma_adc_handler(void) {
ledpwm_update_pwm();
if (sample_at_start && (mc_interface_get_state() == MC_STATE_RUNNING ||
start_comm != mcpwm_get_comm_step())) {
sample_now = 0;
sample_ready = 0;
sample_at_start = 0;
}
static int a = 0;
if (!sample_ready) {
a++;
if (a >= sample_int) {
a = 0;
if (mc_interface_get_state() == MC_STATE_DETECTING) {
curr0_samples[sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
curr1_samples[sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
ph1_samples[sample_now] = (int16_t)mcpwm_detect_voltages[0];
ph2_samples[sample_now] = (int16_t)mcpwm_detect_voltages[1];
ph3_samples[sample_now] = (int16_t)mcpwm_detect_voltages[2];
} else {
curr0_samples[sample_now] = ADC_curr_norm_value[0];
curr1_samples[sample_now] = ADC_curr_norm_value[1];
ph1_samples[sample_now] = ADC_V_L1 - mcpwm_vzero;
ph2_samples[sample_now] = ADC_V_L2 - mcpwm_vzero;
ph3_samples[sample_now] = ADC_V_L3 - mcpwm_vzero;
}
vzero_samples[sample_now] = mcpwm_vzero;
curr_fir_samples[sample_now] = (int16_t)(mc_interface_get_tot_current() * 100.0);
f_sw_samples[sample_now] = (int16_t)(mc_interface_get_switching_frequency_now() / 10.0);
status_samples[sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
sample_now++;
if (sample_now == sample_len) {
sample_ready = 1;
sample_now = 0;
chSysLockFromISR();
chEvtSignalI(sample_send_tp, (eventmask_t) 1);
chSysUnlockFromISR();
}
main_last_adc_duration = mcpwm_get_last_adc_isr_duration();
}
}
}
/**
* @brief Signals all the Event Listeners registered on the specified Event
* Source.
* @details This function variants ORs the specified event flags to all the
* threads registered on the @p EventSource in addition to the event
* flags specified by the threads themselves in the
* @p EventListener objects.
* @post This function does not reschedule so a call to a rescheduling
* function must be performed before unlocking the kernel. Note that
* interrupt handlers always reschedule on exit so an explicit
* reschedule must not be performed in ISRs.
*
* @param[in] esp pointer to the @p EventSource structure
* @param[in] flags the flags set to be added to the listener flags mask
*
* @iclass
*/
void chEvtBroadcastFlagsI(EventSource *esp, flagsmask_t flags) {
EventListener *elp;
chDbgCheckClassI();
chDbgCheck(esp != NULL, "chEvtBroadcastMaskI");
elp = esp->es_next;
while (elp != (EventListener *)esp) {
elp->el_flags |= flags;
chEvtSignalI(elp->el_listener, elp->el_mask);
elp = elp->el_next;
}
}
static void test_002_005_execute(void) {
systime_t time;
eventmask_t events;
/* A set of event flags are set on the current thread then the
function chVTGetSystemTimeX() is invoked, the function is supposed to
return immediately because the event flags are already pending,
after return the events mask is tested.*/
test_set_step(1);
{
time = chVTGetSystemTimeX();
chEvtSignalI(chThdGetSelfX(), 0x55);
events = chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(1000));
test_assert((eventmask_t)0 != events,
"timed out");
test_assert((eventmask_t)0x55 == events,
"wrong events mask");
}
/* The pending event flags mask is cleared then the function
chVTGetSystemTimeX() is invoked, after return the events
mask is tested. The thread is signaled by another thread.*/
test_set_step(2);
{
time = chVTGetSystemTimeX();
chThdGetSelfX()->epmask = 0;
events = chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(1000));
test_assert((eventmask_t)0 != events,
"timed out");
test_assert((eventmask_t)0x55 == events,
"wrong events mask");
}
/* The function chVTGetSystemTimeX() is invoked, no event can
wakeup the thread, the function must return because timeout.*/
test_set_step(3);
{
chSysLock();
time = chVTGetSystemTimeX();
events = chEvtWaitAnyTimeoutS(0, MS2ST(1000));
chSysUnlock();
test_assert_time_window(time + MS2ST(1000),
time + MS2ST(1000) + 1,
"out of time window");
test_assert((eventmask_t)0 == events,
"wrong events mask");
}
}
/**
* @brief Signals all the Event Listeners registered on the specified Event
* Source.
* @details This function variants ORs the specified event flags to all the
* threads registered on the @p event_source_t in addition to the
* event flags specified by the threads themselves in the
* @p event_listener_t objects.
* @post This function does not reschedule so a call to a rescheduling
* function must be performed before unlocking the kernel. Note that
* interrupt handlers always reschedule on exit so an explicit
* reschedule must not be performed in ISRs.
*
* @param[in] esp pointer to the @p event_source_t structure
* @param[in] flags the flags set to be added to the listener flags mask
*
* @iclass
*/
void chEvtBroadcastFlagsI(event_source_t *esp, eventflags_t flags) {
event_listener_t *elp;
chDbgCheckClassI();
chDbgCheck(esp != NULL);
elp = esp->es_next;
while (elp != (event_listener_t *)esp) {
elp->el_flags |= flags;
/* When flags == 0 the thread will always be signaled because the
source does not emit any flag.*/
if ((flags == 0) || ((elp->el_flags & elp->el_wflags) != 0))
chEvtSignalI(elp->el_listener, elp->el_events);
elp = elp->el_next;
}
}
void Keys_t::ITask() {
chThdSleepMilliseconds(KEYS_POLL_PERIOD_MS);
// Check keys
for(uint8_t i=0; i<KEYS_CNT; i++) {
bool PressedNow = PinIsSet(KeyData[i].PGpio, KeyData[i].Pin);
// Check if just pressed
if(PressedNow and !IsPressed[i]) {
chSysLock();
IsPressed[i] = true;
chEvtSignalI(PThd, KeyData[i].EvtMskPress);
chSysUnlock();
}
// Check if just released
else if(!PressedNow and IsPressed[i]) {
IsPressed[i] = false;
}
} // for
}
/**
* @brief Signals all the Event Listeners registered on the specified Event
* Source.
* @details This function variants ORs the specified event flags to all the
* threads registered on the @p event_source_t in addition to the
* event flags specified by the threads themselves in the
* @p event_listener_t objects.
* @post This function does not reschedule so a call to a rescheduling
* function must be performed before unlocking the kernel. Note that
* interrupt handlers always reschedule on exit so an explicit
* reschedule must not be performed in ISRs.
*
* @param[in] esp pointer to the @p event_source_t structure
* @param[in] flags the flags set to be added to the listener flags mask
*
* @iclass
*/
void chEvtBroadcastFlagsI(event_source_t *esp, eventflags_t flags) {
event_listener_t *elp;
chDbgCheckClassI();
chDbgCheck(esp != NULL);
elp = esp->next;
/*lint -save -e9087 -e740 [11.3, 1.3] Cast required by list handling.*/
while (elp != (event_listener_t *)esp) {
/*lint -restore*/
elp->flags |= flags;
/* When flags == 0 the thread will always be signaled because the
source does not emit any flag.*/
if ((flags == (eventflags_t)0) ||
((elp->flags & elp->wflags) != (eventflags_t)0)) {
chEvtSignalI(elp->listener, elp->events);
}
elp = elp->next;
}
}
/*
* This callback is invoked when a character is received but the application
* was not ready to receive it, the character is passed as parameter.
*/
static void rxchar(UARTDriver *uartp, uint16_t c) {
(void)uartp;
/*
* Put the character in a buffer and notify a thread that there is data
* available. An alternative way is to use
*
* packet_process_byte(c);
*
* here directly and skip the thread. However, this could drop bytes if
* processing packets takes a long time.
*/
serial_rx_buffer[serial_rx_write_pos++] = c;
if (serial_rx_write_pos == SERIAL_RX_BUFFER_SIZE) {
serial_rx_write_pos = 0;
}
chEvtSignalI(process_tp, (eventmask_t) 1);
}
void TmrDoseSaveCallback(void *p) {
chSysLockFromIsr();
chEvtSignalI(App.PThd, EVTMSK_DOSE_STORE);
chVTSetI(&ITmrDoseSave, MS2ST(TM_DOSE_SAVE_MS), TmrDoseSaveCallback, nullptr);
chSysUnlockFromIsr();
}
void overflow_cb(ICUDriver *icup) {
chSysLockFromIsr();
chEvtSignalI(thread2, (eventmask_t) 1);
chSysUnlockFromIsr();
}
void period_cb(ICUDriver *icup) {
period = icup->period;
chSysLockFromIsr();
chEvtSignalI(thread1, (eventmask_t) 1);
chSysUnlockFromIsr();
}
/*
* Called every time new ADC values are available. Note that
* the ADC is initialized from mcpwm.c
*/
void main_dma_adc_handler(void) {
ledpwm_update_pwm();
if (sample_at_start && (mcpwm_get_state() == MC_STATE_RUNNING ||
start_comm != mcpwm_get_comm_step())) {
sample_now = 0;
sample_ready = 0;
was_start_sample = 1;
sample_at_start = 0;
}
static int a = 0;
if (!sample_ready) {
a++;
if (a >= sample_int) {
a = 0;
if (mcpwm_get_state() == MC_STATE_DETECTING) {
curr0_samples[sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
curr1_samples[sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
} else {
curr0_samples[sample_now] = ADC_curr_norm_value[0];
curr1_samples[sample_now] = ADC_curr_norm_value[1];
}
ph1_samples[sample_now] = ADC_V_L1 - mcpwm_vzero;
ph2_samples[sample_now] = ADC_V_L2 - mcpwm_vzero;
ph3_samples[sample_now] = ADC_V_L3 - mcpwm_vzero;
vzero_samples[sample_now] = mcpwm_vzero;
curr_fir_samples[sample_now] = (int16_t)(mcpwm_get_tot_current_filtered() * 100);
uint8_t tmp;
if (was_start_sample) {
if (mcpwm_get_state() == MC_STATE_OFF) {
tmp = 1;
} else if (mcpwm_get_state() == MC_STATE_RUNNING) {
tmp = 2;
} else {
tmp = 3;
}
} else {
tmp = mcpwm_read_hall_phase();
}
status_samples[sample_now] = mcpwm_get_comm_step() | (tmp << 3);
sample_now++;
if (sample_now == sample_len) {
sample_ready = 1;
sample_now = 0;
was_start_sample = 0;
chSysLockFromIsr();
chEvtSignalI(sample_send_tp, (eventmask_t) 1);
chSysUnlockFromIsr();
}
main_last_adc_duration = mcpwm_get_last_adc_isr_duration();
}
}
}
// DMA irq
void SIrqDmaHandler(void *p, uint32_t flags) {
chSysLockFromIsr();
chEvtSignalI(Sound.PThread, VS_EVT_DMA_DONE);
chSysUnlockFromIsr();
//Sound.IrqDmaHandler();
}
void Sound_t::ISendNextData() {
// Uart.Printf("sn\r");
IDreq.DisableIrq();
dmaStreamDisable(VS_DMA);
IDmaIdle = false;
// If command queue is not empty, send command
msg_t msg = chMBFetch(&CmdBox, &ICmd.Msg, TIME_IMMEDIATE);
if(msg == RDY_OK) {
// Uart.PrintfI("\rvCmd: %A\r", &ICmd, 4, ' ');
XCS_Lo(); // Start Cmd transmission
chThdSleepMilliseconds(1);
dmaStreamSetMemory0(VS_DMA, &ICmd);
dmaStreamSetTransactionSize(VS_DMA, sizeof(VsCmd_t));
dmaStreamSetMode(VS_DMA, VS_DMA_MODE | STM32_DMA_CR_MINC); // Memory pointer increase
dmaStreamEnable(VS_DMA);
}
// Send next chunk of data if any
else if(State == sndPlaying) {
// Uart.PrintfI("\rD");
// Send data if buffer is not empty
if(PBuf->DataSz != 0) {
XDCS_Lo(); // Start data transmission
uint32_t FLength = (PBuf->DataSz > 32)? 32 : PBuf->DataSz;
dmaStreamSetMemory0(VS_DMA, PBuf->PData);
dmaStreamSetTransactionSize(VS_DMA, FLength);
dmaStreamSetMode(VS_DMA, VS_DMA_MODE | STM32_DMA_CR_MINC); // Memory pointer increase
dmaStreamEnable(VS_DMA);
// Process pointers and lengths
PBuf->DataSz -= FLength;
PBuf->PData += FLength;
}
else IDmaIdle = true; // Will come true if both buffers are empty
// Check if buffer is now empty
if(PBuf->DataSz == 0) {
// Prepare to read next chunk
// Uart.Printf("*");
chSysLock();
chEvtSignalI(PThread, VS_EVT_READ_NEXT);
chSysUnlock();
// Switch to next buf
PBuf = (PBuf == &Buf1)? &Buf2 : &Buf1;
}
}
else if(State == sndWritingZeroes) {
// Uart.Printf("\rZ");
if(ZeroesCount == 0) { // Was writing zeroes, now all over
State = sndStopped;
IDmaIdle = true;
// Uart.Printf("vEnd\r");
chSysLock();
chEvtSignalI(PThread, VS_EVT_COMPLETED);
chSysUnlock();
}
else SendZeroes();
}
else {
// Uart.PrintfI("\rI");
if(!IDreq.IsHi()) IDreq.EnableIrq(IRQ_PRIO_MEDIUM);
else IDmaIdle = true;
}
}
// Universal VirtualTimer callback
void TmrGeneralCallback(void *p) {
chSysLockFromIsr();
chEvtSignalI(App.PThd, (eventmask_t)p);
chSysUnlockFromIsr();
}
void mc_interface_mc_timer_isr(void) {
ledpwm_update_pwm(); // LED PWM Driver update
const float input_voltage = GET_INPUT_VOLTAGE();
// Check for faults that should stop the motor
static int wrong_voltage_iterations = 0;
if (input_voltage < m_conf.l_min_vin ||
input_voltage > m_conf.l_max_vin) {
wrong_voltage_iterations++;
if ((wrong_voltage_iterations >= 8)) {
mc_interface_fault_stop(input_voltage < m_conf.l_min_vin ?
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE);
}
} else {
wrong_voltage_iterations = 0;
}
if (mc_interface_get_state() == MC_STATE_RUNNING) {
m_cycles_running++;
} else {
m_cycles_running = 0;
}
if (pwn_done_func) {
pwn_done_func();
}
const float current = mc_interface_get_tot_current_filtered();
const float current_in = mc_interface_get_tot_current_in_filtered();
m_motor_current_sum += current;
m_input_current_sum += current_in;
m_motor_current_iterations++;
m_input_current_iterations++;
float abs_current = mc_interface_get_tot_current();
float abs_current_filtered = current;
if (m_conf.motor_type == MOTOR_TYPE_FOC) {
// TODO: Make this more general
abs_current = mcpwm_foc_get_abs_motor_current();
abs_current_filtered = mcpwm_foc_get_abs_motor_current_filtered();
}
// Current fault code
if (m_conf.l_slow_abs_current) {
if (fabsf(abs_current_filtered) > m_conf.l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
} else {
if (fabsf(abs_current) > m_conf.l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
}
// Watt and ah counters
const float f_sw = mc_interface_get_switching_frequency_now();
if (fabsf(current) > 1.0) {
// Some extra filtering
static float curr_diff_sum = 0.0;
static float curr_diff_samples = 0;
curr_diff_sum += current_in / f_sw;
curr_diff_samples += 1.0 / f_sw;
if (curr_diff_samples >= 0.01) {
if (curr_diff_sum > 0.0) {
m_amp_seconds += curr_diff_sum;
m_watt_seconds += curr_diff_sum * input_voltage;
} else {
m_amp_seconds_charged -= curr_diff_sum;
m_watt_seconds_charged -= curr_diff_sum * input_voltage;
}
curr_diff_samples = 0.0;
curr_diff_sum = 0.0;
}
}
// Sample collection
if (m_sample_at_start && (mc_interface_get_state() == MC_STATE_RUNNING ||
m_start_comm != mcpwm_get_comm_step())) {
m_sample_now = 0;
m_sample_ready = 0;
m_sample_at_start = 0;
}
static int a = 0;
if (!m_sample_ready) {
a++;
if (a >= m_sample_int) {
a = 0;
if (mc_interface_get_state() == MC_STATE_DETECTING) {
m_curr0_samples[m_sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
m_curr1_samples[m_sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
m_ph1_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[0];
m_ph2_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[1];
m_ph3_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[2];
} else {
m_curr0_samples[m_sample_now] = ADC_curr_norm_value[0];
m_curr1_samples[m_sample_now] = ADC_curr_norm_value[1];
m_ph1_samples[m_sample_now] = ADC_V_L1 - mcpwm_vzero;
m_ph2_samples[m_sample_now] = ADC_V_L2 - mcpwm_vzero;
m_ph3_samples[m_sample_now] = ADC_V_L3 - mcpwm_vzero;
}
m_vzero_samples[m_sample_now] = mcpwm_vzero;
m_curr_fir_samples[m_sample_now] = (int16_t)(mc_interface_get_tot_current() * 100.0);
m_f_sw_samples[m_sample_now] = (int16_t)(mc_interface_get_switching_frequency_now() / 10.0);
m_status_samples[m_sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
m_sample_now++;
if (m_sample_now == m_sample_len) {
m_sample_ready = 1;
m_sample_now = 0;
chSysLockFromISR();
chEvtSignalI(sample_send_tp, (eventmask_t) 1);
chSysUnlockFromISR();
}
m_last_adc_duration_sample = mcpwm_get_last_adc_isr_duration();
}
}
}
void Sound_t::ISendNextData() {
// Uart.Printf("\rSN");
dmaStreamDisable(VS_DMA);
IDmaIdle = false;
// ==== If command queue is not empty, send command ====
msg_t msg = chMBFetch(&CmdBox, &ICmd.Msg, TIME_IMMEDIATE);
if(msg == RDY_OK) {
// Uart.PrintfI("\rvCmd: %A", &ICmd, 4, ' ');
XCS_Lo(); // Start Cmd transmission
dmaStreamSetMemory0(VS_DMA, &ICmd);
dmaStreamSetTransactionSize(VS_DMA, sizeof(VsCmd_t));
dmaStreamSetMode(VS_DMA, VS_DMA_MODE | STM32_DMA_CR_MINC); // Memory pointer increase
dmaStreamEnable(VS_DMA);
}
// ==== Send next chunk of data if any ====
else switch(State) {
case sndPlaying: {
// Uart.PrintfI("\rD");
// Switch buffer if required
if(PBuf->DataSz == 0) {
PBuf = (PBuf == &Buf1)? &Buf2 : &Buf1; // Switch to next buf
// Uart.Printf("\rB=%u; Sz=%u", ((PBuf == &Buf1)? 1 : 2), PBuf->DataSz);
if(PBuf->DataSz == 0) { // Previous attempt to read the file failed
IDmaIdle = true;
PrepareToStop();
break;
}
else {
chSysLock();
chEvtSignalI(PThread, VS_EVT_READ_NEXT); // Read next chunk of file
chSysUnlock();
}
}
// Send next piece of data
XDCS_Lo(); // Start data transmission
uint32_t FLength = (PBuf->DataSz > 32)? 32 : PBuf->DataSz;
dmaStreamSetMemory0(VS_DMA, PBuf->PData);
dmaStreamSetTransactionSize(VS_DMA, FLength);
dmaStreamSetMode(VS_DMA, VS_DMA_MODE | STM32_DMA_CR_MINC); // Memory pointer increase
dmaStreamEnable(VS_DMA);
// if(PBuf == &Buf1) Uart.Printf("*"); else Uart.Printf("#");
// Process pointers and lengths
PBuf->DataSz -= FLength;
PBuf->PData += FLength;
} break;
case sndWritingZeroes:
// Uart.Printf("\rZ");
if(ZeroesCount == 0) { // Was writing zeroes, now all over
State = sndStopped;
IDmaIdle = true;
// Uart.Printf("\rvEnd");
chSysLock();
chEvtSignalI(PThread, VS_EVT_COMPLETED);
chSysUnlock();
}
else SendZeroes();
break;
case sndStopped:
// Uart.PrintfI("\rI");
if(!IDreq.IsHi()) IDreq.EnableIrq(IRQ_PRIO_MEDIUM);
else IDmaIdle = true;
} // switch
}
static inline void events_flag_isr(const eventmask_t events) {
if( thread_event_loop ) {
chEvtSignalI(thread_event_loop, events);
}
}
void TmrPillCheckCallback(void *p) {
chSysLockFromIsr();
chEvtSignalI(App.PThd, EVTMSK_PILL_CHECK);
chVTSetI(&ITmrPillCheck, MS2ST(TM_PILL_CHECK_MS), TmrPillCheckCallback, nullptr);
chSysUnlockFromIsr();
}
void TmrUartRxCallback(void *p) {
chSysLockFromIsr();
chEvtSignalI(App.PThread, EVTMSK_UART_RX_POLL);
chVTSetI(&App.TmrUartRx, MS2ST(UART_RX_POLLING_MS), TmrUartRxCallback, nullptr);
chSysUnlockFromIsr();
}
bool AP_IOMCU_FW::handle_code_write()
{
switch (rx_io_packet.page) {
case PAGE_SETUP:
switch (rx_io_packet.offset) {
case PAGE_REG_SETUP_ARMING:
reg_setup.arming = rx_io_packet.regs[0];
break;
case PAGE_REG_SETUP_FORCE_SAFETY_OFF:
if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
hal.rcout->force_safety_off();
reg_status.flag_safety_off = true;
} else {
return false;
}
break;
case PAGE_REG_SETUP_FORCE_SAFETY_ON:
if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
hal.rcout->force_safety_on();
reg_status.flag_safety_off = false;
} else {
return false;
}
break;
case PAGE_REG_SETUP_ALTRATE:
reg_setup.pwm_altrate = rx_io_packet.regs[0];
update_rcout_freq = true;
break;
case PAGE_REG_SETUP_PWM_RATE_MASK:
reg_setup.pwm_rates = rx_io_packet.regs[0];
update_rcout_freq = true;
break;
case PAGE_REG_SETUP_DEFAULTRATE:
if (rx_io_packet.regs[0] < 25 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 25;
}
if (rx_io_packet.regs[0] > 400 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 400;
}
reg_setup.pwm_defaultrate = rx_io_packet.regs[0];
update_default_rate = true;
break;
case PAGE_REG_SETUP_SBUS_RATE:
reg_setup.sbus_rate = rx_io_packet.regs[0];
sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
break;
case PAGE_REG_SETUP_FEATURES:
reg_setup.features = rx_io_packet.regs[0];
/* disable the conflicting options with SBUS 1 */
if (reg_setup.features & (P_SETUP_FEATURES_SBUS1_OUT)) {
reg_setup.features &= ~(P_SETUP_FEATURES_PWM_RSSI |
P_SETUP_FEATURES_ADC_RSSI |
P_SETUP_FEATURES_SBUS2_OUT);
// enable SBUS output at specified rate
sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
palClearLine(HAL_GPIO_PIN_SBUS_OUT_EN);
} else {
palSetLine(HAL_GPIO_PIN_SBUS_OUT_EN);
}
break;
case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
reg_setup.heater_duty_cycle = rx_io_packet.regs[0];
last_heater_ms = AP_HAL::millis();
break;
case PAGE_REG_SETUP_REBOOT_BL:
if (reg_status.flag_safety_off) {
// don't allow reboot while armed
return false;
}
// check the magic value
if (rx_io_packet.regs[0] != REBOOT_BL_MAGIC) {
return false;
}
schedule_reboot(100);
break;
default:
break;
}
break;
case PAGE_DIRECT_PWM: {
/* copy channel data */
uint8_t i = 0, offset = rx_io_packet.offset, num_values = rx_io_packet.count;
while ((offset < IOMCU_MAX_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
if (rx_io_packet.regs[i] != PWM_IGNORE_THIS_CHANNEL) {
reg_direct_pwm.pwm[offset] = rx_io_packet.regs[i];
}
offset++;
num_values--;
i++;
}
fmu_data_received_time = AP_HAL::millis();
reg_status.flag_fmu_ok = true;
reg_status.flag_raw_pwm = true;
chEvtSignalI(thread_ctx, EVENT_MASK(IOEVENT_PWM));
break;
}
default:
break;
}
memset(&tx_io_packet, 0xFF, sizeof(tx_io_packet));
tx_io_packet.count = 0;
tx_io_packet.code = CODE_SUCCESS;
tx_io_packet.crc = 0;
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
return true;
}
