/**
* @brief Initializes the kernel.
* @details Initializes the kernel structures, the current instructions flow
* becomes the idle thread upon return. The idle thread must not
* invoke any kernel primitive able to change state to not runnable.
* @note This function assumes that the @p nil global variable has been
* zeroed by the runtime environment. If this is not the case then
* make sure to clear it before calling this function.
*
* @special
*/
void chSysInit(void) {
thread_t *tp;
const thread_config_t *tcp;
#if CH_DBG_SYSTEM_STATE_CHECK == TRUE
nil.isr_cnt = (cnt_t)0;
nil.lock_cnt = (cnt_t)0;
#endif
/* System initialization hook.*/
CH_CFG_SYSTEM_INIT_HOOK();
/* Iterates through the list of defined threads.*/
tp = &nil.threads[0];
tcp = nil_thd_configs;
while (tp < &nil.threads[CH_CFG_NUM_THREADS]) {
#if CH_DBG_ENABLE_STACK_CHECK
tp->stklimit = (stkalign_t *)tcp->wbase;
#endif
/* Port dependent thread initialization.*/
PORT_SETUP_CONTEXT(tp, tcp->wbase, tcp->wend, tcp->funcp, tcp->arg);
/* Initialization hook.*/
CH_CFG_THREAD_EXT_INIT_HOOK(tp);
tp++;
tcp++;
}
#if CH_DBG_ENABLE_STACK_CHECK
/* The idle thread is a special case because its stack is set up by the
runtime environment.*/
tp->stklimit = THD_IDLE_BASE;
#endif
/* Interrupts partially enabled. It is equivalent to entering the
kernel critical zone.*/
chSysSuspend();
#if CH_DBG_SYSTEM_STATE_CHECK == TRUE
nil.lock_cnt = (cnt_t)1;
#endif
/* Heap initialization, if enabled.*/
#if CH_CFG_USE_HEAP == TRUE
_heap_init();
#endif
/* Port layer initialization last because it depend on some of the
initializations performed before.*/
port_init();
/* Runs the highest priority thread, the current one becomes the idle
thread.*/
nil.current = nil.next = nil.threads;
port_switch(nil.current, tp);
chSysUnlock();
}
CriticalSectionLocker() { chSysSuspend(); }
void motor_pwm_beep(int frequency, int duration_msec)
{
static const float DUTY_CYCLE = 0.01;
static const int ACTIVE_USEC_MAX = 20;
motor_pwm_set_freewheeling();
frequency = (frequency < 100) ? 100 : frequency;
frequency = (frequency > 5000) ? 5000 : frequency;
duration_msec = (duration_msec < 1) ? 1 : duration_msec;
duration_msec = (duration_msec > 5000) ? 5000 : duration_msec;
/*
* Timing constants
*/
const int half_period_hnsec = (HNSEC_PER_SEC / frequency) / 2;
int active_hnsec = half_period_hnsec * DUTY_CYCLE;
if (active_hnsec > ACTIVE_USEC_MAX * HNSEC_PER_USEC) {
active_hnsec = ACTIVE_USEC_MAX * HNSEC_PER_USEC;
}
const int idle_hnsec = half_period_hnsec - active_hnsec;
const uint64_t end_time = motor_timer_hnsec() + duration_msec * HNSEC_PER_MSEC;
/*
* FET round robin
* This way we can beep even if some FETs went bananas
*/
static unsigned _phase_sel;
const int low_phase_first = _phase_sel++ % MOTOR_NUM_PHASES;
const int low_phase_second = _phase_sel++ % MOTOR_NUM_PHASES;
const int high_phase = _phase_sel++ % MOTOR_NUM_PHASES;
_phase_sel++; // We need to increment it not by multiple of 3
assert(low_phase_first != high_phase && low_phase_second != high_phase);
/*
* Commutations
* No high side pumping
*/
phase_set_i(low_phase_first, 0, false);
phase_set_i(low_phase_second, 0, false);
phase_set_i(high_phase, 0, false);
apply_phase_config();
while (end_time > motor_timer_hnsec()) {
chSysSuspend();
irq_primask_disable();
phase_set_i(high_phase, _pwm_top, false);
apply_phase_config();
irq_primask_enable();
motor_timer_hndelay(active_hnsec);
irq_primask_disable();
phase_set_i(high_phase, 0, false);
apply_phase_config();
irq_primask_enable();
chSysEnable();
motor_timer_hndelay(idle_hnsec);
}
motor_pwm_set_freewheeling();
}
