void motor_rtctl_start(float duty_cycle, bool reverse)
{
motor_rtctl_stop(); // Just in case
if (!_initialization_confirmed) {
return; // Go home you're drunk
}
if (duty_cycle <= 0) {
assert(0);
return;
}
/*
* Initialize the structs
*/
memset(&_diag, 0, sizeof(_diag));
memset(&_state, 0, sizeof(_state)); // Mighty reset
motor_forced_rotation_detector_reset();
_diag.started_at = motor_timer_hnsec();
_state.comm_table = reverse ? COMMUTATION_TABLE_REVERSE : COMMUTATION_TABLE_FORWARD;
_state.pwm_val_after_spinup = motor_pwm_compute_pwm_val(duty_cycle);
init_adc_filters();
/*
* Initial spinup.
*/
const tprio_t orig_priority = chThdSetPriority(HIGHPRIO);
motor_pwm_prepare_to_start();
const bool started = do_bemf_spinup(duty_cycle);
/*
* Engage the normal mode if started
* At this point, if the spinup was OK, we're sutiated RIGHT AFTER THE DETECTED ZERO CROSS, engaged.
*/
if (started) {
_state.blank_time_deadline = motor_timer_hnsec() + _params.comm_blank_hnsec;
_state.zc_detection_result = ZC_DETECTED;
_state.flags = FLAG_ACTIVE | FLAG_SPINUP;
motor_timer_set_relative(_state.comm_period / 3);
lowsyslog("Motor: Spinup OK, comm period: %u usec\n", (unsigned)(_state.comm_period / HNSEC_PER_USEC));
} else {
lowsyslog("Motor: Spinup failed\n");
motor_rtctl_stop();
}
chThdSetPriority(orig_priority);
}
bool fiveBaudInit(SerialDriver *sd)
{
uint8_t input_buf[3];
size_t bytes;
// Set pin mode to GPIO
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_RX, PAL_MODE_INPUT_ANALOG);
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_TX, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
const uint8_t prio = chThdGetPriorityX();
chThdSetPriority(HIGHPRIO);
K_HIGH(320); // As per ISO standard
// Send 0x33 at 5 baud (00110011)
// K-line level: |_--__--__-|
K_LOW(200); // Low for 200ms - start bit
K_HIGH(400); // High for 400ms - 00
K_LOW(400); // Low for 400ms - 11
K_HIGH(400); // High for 400ms - 00
K_LOW(400); // Low for 400ms - 11
K_HIGH(200); // High for 200ms - stop bit
// Set pin mode back to UART
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_TX, PAL_MODE_ALTERNATE(7) | \
PAL_STM32_OSPEED_HIGHEST | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_PUPDR_PULLUP);
palSetPadMode(PORT_KLINE_TX, PAD_KLINE_RX, PAL_MODE_ALTERNATE(7) | \
PAL_STM32_OTYPE_OPENDRAIN);
chThdSetPriority(prio); // Revert back original priority
chThdSleepMilliseconds(25);
bytes = sdReadTimeout(sd, input_buf, sizeof(input_buf), MS2ST(500)); // 300ms max according to ISO9141
if (bytes != 3 || input_buf[0] != 0x55)
{
return false;
}
chThdSleepMilliseconds(35); // 25-50 ms pause per ISO standard
uint8_t key = input_buf[2] ^ 0xFF; // Invert key byte
sdWriteTimeout(sd, &key, 1, MS2ST(100));
chThdSleepMilliseconds(35); // 25-50 ms pause per ISO standard
bytes = sdReadTimeout(sd, input_buf, 1, MS2ST(100));
if (bytes != 1 || input_buf[0] != 0xCC)
{
return false;
}
return true;
}
int main(void)
{
halInit();
/* Initialize ChibiOS core */
chSysInit();
sc_init(SC_MODULE_LED);
// Start event loop. This will start a new thread and return
sc_event_loop_start();
#if defined CH_NO_IDLE_THREAD && CH_NO_IDLE_THREAD
chThdSetPriority(IDLEPRIO);
#endif
// Loop forever waiting for callbacks
while(1) {
sc_led_toggle();
#if defined CH_NO_IDLE_THREAD && CH_NO_IDLE_THREAD
chDbgAssert(0, "Can't call chThdSleepMilliseconds() without idle thread");
#endif
chThdSleepMilliseconds(1000);
}
return 0;
}
static void thd3_execute(void) {
tprio_t prio, p1;
prio = chThdGetPriority();
p1 = chThdSetPriority(prio + 1);
test_assert(1, p1 == prio,
"unexpected returned priority level");
test_assert(2, chThdGetPriority() == prio + 1,
"unexpected priority level");
p1 = chThdSetPriority(p1);
test_assert(3, p1 == prio + 1,
"unexpected returned priority level");
test_assert(4, chThdGetPriority() == prio,
"unexpected priority level");
#if CH_USE_MUTEXES || defined(__DOXYGEN__)
/* Simulates a priority boost situation (p_prio > p_realprio).*/
chSysLock();
chThdSelf()->p_prio += 2;
chSysUnlock();
test_assert(5, chThdGetPriority() == prio + 2,
"unexpected priority level");
/* Tries to raise but below the boost level. */
p1 = chThdSetPriority(prio + 1);
test_assert(6, p1 == prio,
"unexpected returned priority level");
test_assert(7, chThdSelf()->p_prio == prio + 2,
"unexpected priority level");
test_assert(8, chThdSelf()->p_realprio == prio + 1,
"unexpected returned real priority level");
/* Tries to raise above the boost level. */
p1 = chThdSetPriority(prio + 3);
test_assert(9, p1 == prio + 1,
"unexpected returned priority level");
test_assert(10, chThdSelf()->p_prio == prio + 3,
"unexpected priority level");
test_assert(11, chThdSelf()->p_realprio == prio + 3,
"unexpected real priority level");
chSysLock();
chThdSelf()->p_prio = prio;
chThdSelf()->p_realprio = prio;
chSysUnlock();
#endif
}
/**
* @brief Kernel start.
*/
osStatus osKernelStart(void) {
cmsis_os_started = 1;
chThdSetPriority(NORMALPRIO);
return osOK;
}
static void test_002_004_execute(void) {
tprio_t prio, p1;
/* [2.4.1] Simulating a priority boost situation (prio > realprio).*/
test_set_step(1);
{
prio = chThdGetPriorityX();
chThdGetSelfX()->prio += 2;
test_assert(chThdGetPriorityX() == prio + 2, "unexpected priority level");
}
/* [2.4.2] Raising thread priority above original priority but below
the boosted level.*/
test_set_step(2);
{
p1 = chThdSetPriority(prio + 1);
test_assert(p1 == prio, "unexpected returned priority level");
test_assert(chThdGetSelfX()->prio == prio + 2, "unexpected priority level");
test_assert(chThdGetSelfX()->realprio == prio + 1, "unexpected returned real priority level");
}
/* [2.4.3] Raising thread priority above the boosted level.*/
test_set_step(3);
{
p1 = chThdSetPriority(prio + 3);
test_assert(p1 == prio + 1, "unexpected returned priority level");
test_assert(chThdGetSelfX()->prio == prio + 3, "unexpected priority level");
test_assert(chThdGetSelfX()->realprio == prio + 3, "unexpected real priority level");
}
/* [2.4.4] Restoring original conditions.*/
test_set_step(4);
{
chSysLock();
chThdGetSelfX()->prio = prio;
chThdGetSelfX()->realprio = prio;
chSysUnlock();
}
}
static void test_002_003_execute(void) {
tprio_t prio, p1;
/* [2.3.1] Thread priority is increased by one then a check is
performed.*/
test_set_step(1);
{
prio = chThdGetPriorityX();
p1 = chThdSetPriority(prio + 1);
test_assert(p1 == prio, "unexpected returned priority level");
test_assert(chThdGetPriorityX() == prio + 1, "unexpected priority level");
}
/* [2.3.2] Thread priority is returned to the previous value then a
check is performed.*/
test_set_step(2);
{
p1 = chThdSetPriority(p1);
test_assert(p1 == prio + 1, "unexpected returned priority level");
test_assert(chThdGetPriorityX() == prio, "unexpected priority level");
}
}
/**
* @brief Kernel initialization.
*/
osStatus osKernelInitialize(void) {
cmsis_os_started = 0;
chSysInit();
chThdSetPriority(HIGHPRIO);
chPoolObjectInit(&sempool, sizeof(semaphore_t), chCoreAlloc);
chPoolLoadArray(&sempool, semaphores, CMSIS_CFG_NUM_SEMAPHORES);
chPoolObjectInit(&timpool, sizeof(virtual_timer_t), chCoreAlloc);
chPoolLoadArray(&timpool, timers, CMSIS_CFG_NUM_TIMERS);
return osOK;
}
void motor_rtctl_beep(int frequency, int duration_msec)
{
if (_state.flags & FLAG_ACTIVE) {
return;
}
irq_primask_disable();
motor_adc_disable_from_isr();
irq_primask_enable();
const tprio_t orig_priority = chThdSetPriority(HIGHPRIO);
motor_pwm_beep(frequency, duration_msec);
chThdSetPriority(orig_priority);
irq_primask_disable();
motor_adc_enable_from_isr();
irq_primask_enable();
/*
* Motor windings may get saturated after beeping, making immediately following spinup unreliable.
* This little delay fixes that, not in the best way though.
*/
usleep(10000);
}
int main(void)
{
halInit();
chibios_rt::System::Init();
chRegSetThreadName("Main");
chThdSleepMilliseconds(1000);
//init wireless module
new Wireless;
chThdSetPriority(NORMALPRIO - 1);
while (TRUE)
{
delay_process::Play();
chThdSleepMilliseconds(1);
}
}
int main(void) {
halInit();
chSysInit();
chRegSetThreadName("main");
LocalStatus = &M2RStatus;
/*sbp_state_init(&sbp_state);*/
rockblock_init();
dispatch_init();
chThdCreateStatic(waThreadHB, sizeof(waThreadHB), LOWPRIO,
ThreadHeartbeat, NULL);
/* Configure and enable the watchdog timer, stopped in debug halt */
DBGMCU->APB1FZ |= DBGMCU_APB1_FZ_DBG_IWDG_STOP;
IWDG->KR = 0x5555;
IWDG->PR = 3;
IWDG->KR = 0xCCCC;
m2serial_shell = m2r_shell_run;
M2SerialSD = &SD2;
chThdCreateStatic(waM2Serial, sizeof(waM2Serial), HIGHPRIO,
m2serial_thread, NULL);
chThdCreateStatic(waM2Status, sizeof(waM2Status), HIGHPRIO,
m2status_thread, NULL);
chThdCreateStatic(waThreadUblox, sizeof(waThreadUblox), NORMALPRIO,
ublox_thread, NULL);
chThdCreateStatic(waThreadRadio, sizeof(waThreadRadio), NORMALPRIO,
radio_thread, NULL);
chThdCreateStatic(waDispatch, sizeof(waDispatch), NORMALPRIO,
dispatch_thread, NULL);
chThdSetPriority(LOWPRIO);
chThdSleep(TIME_INFINITE);
return 0;
}
/**
* @brief LWIP handling thread.
*
* @param[in] p pointer to a @p lwipthread_opts structure or @p NULL
* @return The function does not return.
*/
msg_t lwip_thread(void *p) {
event_timer_t evt;
event_listener_t el0, el1;
struct ip_addr ip, gateway, netmask;
static struct netif thisif;
static const MACConfig mac_config = {thisif.hwaddr};
chRegSetThreadName("lwipthread");
/* Initializes the thing.*/
tcpip_init(NULL, NULL);
/* TCP/IP parameters, runtime or compile time.*/
if (p) {
struct lwipthread_opts *opts = p;
unsigned i;
for (i = 0; i < 6; i++)
thisif.hwaddr[i] = opts->macaddress[i];
ip.addr = opts->address;
gateway.addr = opts->gateway;
netmask.addr = opts->netmask;
}
else {
thisif.hwaddr[0] = LWIP_ETHADDR_0;
thisif.hwaddr[1] = LWIP_ETHADDR_1;
thisif.hwaddr[2] = LWIP_ETHADDR_2;
thisif.hwaddr[3] = LWIP_ETHADDR_3;
thisif.hwaddr[4] = LWIP_ETHADDR_4;
thisif.hwaddr[5] = LWIP_ETHADDR_5;
LWIP_IPADDR(&ip);
LWIP_GATEWAY(&gateway);
LWIP_NETMASK(&netmask);
}
macStart(ÐD1, &mac_config);
netif_add(&thisif, &ip, &netmask, &gateway, NULL, ethernetif_init, tcpip_input);
netif_set_default(&thisif);
netif_set_up(&thisif);
/* Setup event sources.*/
evtObjectInit(&evt, LWIP_LINK_POLL_INTERVAL);
evtStart(&evt);
chEvtRegisterMask(&evt.et_es, &el0, PERIODIC_TIMER_ID);
chEvtRegisterMask(macGetReceiveEventSource(ÐD1), &el1, FRAME_RECEIVED_ID);
chEvtAddEvents(PERIODIC_TIMER_ID | FRAME_RECEIVED_ID);
/* Goes to the final priority after initialization.*/
chThdSetPriority(LWIP_THREAD_PRIORITY);
while (TRUE) {
eventmask_t mask = chEvtWaitAny(ALL_EVENTS);
if (mask & PERIODIC_TIMER_ID) {
bool_t current_link_status = macPollLinkStatus(ÐD1);
if (current_link_status != netif_is_link_up(&thisif)) {
if (current_link_status)
tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_up,
&thisif, 0);
else
tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_down,
&thisif, 0);
}
}
if (mask & FRAME_RECEIVED_ID) {
struct pbuf *p;
while ((p = low_level_input(&thisif)) != NULL) {
struct eth_hdr *ethhdr = p->payload;
switch (htons(ethhdr->type)) {
/* IP or ARP packet? */
case ETHTYPE_IP:
case ETHTYPE_ARP:
#if PPPOE_SUPPORT
/* PPPoE packet? */
case ETHTYPE_PPPOEDISC:
case ETHTYPE_PPPOE:
#endif /* PPPOE_SUPPORT */
/* full packet send to tcpip_thread to process */
if (thisif.input(p, &thisif) == ERR_OK)
break;
LWIP_DEBUGF(NETIF_DEBUG, ("ethernetif_input: IP input error\n"));
default:
pbuf_free(p);
}
}
}
}
return 0;
}
/*
* M2FC Main Thread.
* Starts all the other threads then puts itself to sleep.
*/
int main(void) {
halInit();
chSysInit();
chRegSetThreadName("Main");
/* Start the heartbeat thread so it will be resetting the watchdog. */
chThdCreateStatic(waThreadHB, sizeof(waThreadHB), LOWPRIO,
ThreadHeartbeat, NULL);
/* Configure and enable the watchdog timer, stopped in debug halt. */
DBGMCU->APB1FZ |= DBGMCU_APB1_FZ_DBG_IWDG_STOP;
IWDG->KR = 0x5555;
IWDG->PR = 3;
IWDG->KR = 0xCCCC;
/* Various module initialisation */
state_estimation_init();
dma_mutexes_init();
/* Read config from SD card and wait for completion. */
Thread* cfg_tp = chThdCreateStatic(waConfig, sizeof(waConfig),
NORMALPRIO, config_thread, NULL);
while(cfg_tp->p_state != THD_STATE_FINAL) chThdSleepMilliseconds(10);
if(conf.location == CFG_M2FC_BODY)
LocalStatus = &M2FCBodyStatus;
else if(conf.location == CFG_M2FC_NOSE)
LocalStatus = &M2FCNoseStatus;
if(conf.config_loaded)
m2status_config_status(STATUS_OK);
else
m2status_config_status(STATUS_ERR);
/* Activate the EXTI pin change interrupts */
extStart(&EXTD1, &extcfg);
/* Start module threads */
m2serial_shell = m2fc_shell_run;
M2SerialSD = &SD1;
sdStart(M2SerialSD, NULL);
chThdCreateStatic(waM2Serial, sizeof(waM2Serial), HIGHPRIO,
m2serial_thread, NULL);
chThdCreateStatic(waM2Status, sizeof(waM2Status), HIGHPRIO,
m2status_thread, NULL);
chThdCreateStatic(waDatalogging, sizeof(waDatalogging), HIGHPRIO,
datalogging_thread, NULL);
chThdCreateStatic(waMission, sizeof(waMission), NORMALPRIO,
mission_thread, NULL);
chThdCreateStatic(waMS5611, sizeof(waMS5611), NORMALPRIO,
ms5611_thread, NULL);
chThdCreateStatic(waADXL345, sizeof(waADXL345), NORMALPRIO,
adxl345_thread, NULL);
chThdCreateStatic(waADXL375, sizeof(waADXL375), NORMALPRIO,
adxl375_thread, NULL);
chThdCreateStatic(waPyros, sizeof(waPyros), NORMALPRIO,
pyro_continuity_thread, NULL);
if(conf.use_gyro) {
chThdCreateStatic(waL3G4200D, sizeof(waL3G4200D), NORMALPRIO,
l3g4200d_thread, NULL);
} else {
m2status_gyro_status(STATUS_OK);
}
if(conf.use_magno) {
chThdCreateStatic(waHMC5883L, sizeof(waHMC5883L), NORMALPRIO,
hmc5883l_thread, NULL);
} else {
m2status_magno_status(STATUS_OK);
}
if(conf.use_adc) {
chThdCreateStatic(waAnalogue, sizeof(waAnalogue), NORMALPRIO,
analogue_thread, NULL);
} else {
m2status_adc_status(STATUS_OK);
}
/* Let the main thread idle now. */
chThdSetPriority(LOWPRIO);
chThdSleep(TIME_INFINITE);
return 0;
}
tprio_t BaseThread::setPriority(tprio_t newprio) {
return chThdSetPriority(newprio);
}
static void main_loop()
{
daemon_task = chThdGetSelfX();
/*
switch to high priority for main loop
*/
chThdSetPriority(APM_MAIN_PRIORITY);
#ifdef HAL_I2C_CLEAR_BUS
// Clear all I2C Buses. This can be needed on some boards which
// can get a stuck I2C peripheral on boot
ChibiOS::I2CBus::clear_all();
#endif
#if STM32_DMA_ADVANCED
ChibiOS::Shared_DMA::init();
#endif
peripheral_power_enable();
hal.uartA->begin(115200);
#ifdef HAL_SPI_CHECK_CLOCK_FREQ
// optional test of SPI clock frequencies
ChibiOS::SPIDevice::test_clock_freq();
#endif
hal.uartB->begin(38400);
hal.uartC->begin(57600);
hal.analogin->init();
hal.scheduler->init();
/*
run setup() at low priority to ensure CLI doesn't hang the
system, and to allow initial sensor read loops to run
*/
hal_chibios_set_priority(APM_STARTUP_PRIORITY);
schedulerInstance.hal_initialized();
g_callbacks->setup();
hal.scheduler->system_initialized();
thread_running = true;
chRegSetThreadName(SKETCHNAME);
/*
switch to high priority for main loop
*/
chThdSetPriority(APM_MAIN_PRIORITY);
while (true) {
g_callbacks->loop();
/*
give up 50 microseconds of time if the INS loop hasn't
called delay_microseconds_boost(), to ensure low priority
drivers get a chance to run. Calling
delay_microseconds_boost() means we have already given up
time from the main loop, so we don't need to do it again
here
*/
#ifndef HAL_DISABLE_LOOP_DELAY
if (!schedulerInstance.check_called_boost()) {
hal.scheduler->delay_microseconds(50);
}
#endif
}
thread_running = false;
}
static msg_t control_thread(void* arg)
{
(void)arg;
chRegSetThreadName("motor");
EventListener listener;
chEvtRegisterMask(&_setpoint_update_event, &listener, ALL_EVENTS);
uint64_t timestamp_hnsec = motor_rtctl_timestamp_hnsec();
while (1) {
/*
* Control loop period adapts to comm period.
*/
const uint32_t comm_period = motor_rtctl_get_comm_period_hnsec();
unsigned control_period_ms = IDLE_CONTROL_PERIOD_MSEC;
if (comm_period > 0) {
control_period_ms = comm_period / HNSEC_PER_MSEC;
}
if (control_period_ms < 1) {
control_period_ms = 1;
} else if (control_period_ms > IDLE_CONTROL_PERIOD_MSEC) {
control_period_ms = IDLE_CONTROL_PERIOD_MSEC;
}
/*
* Thread priority - maximum if the motor is running, normal otherwise
*/
const tprio_t desired_thread_priority = (comm_period > 0) ? HIGHPRIO : NORMALPRIO;
if (desired_thread_priority != chThdGetPriority()) {
const tprio_t old = chThdSetPriority(desired_thread_priority);
lowsyslog("Motor: Priority changed: %i --> %i\n", (int)old, (int)desired_thread_priority);
}
/*
* The event must be set only when the mutex is unlocked.
* Otherwise this thread will take control, stumble upon the locked mutex, return the control
* to the thread that holds the mutex, unlock the mutex, then proceed.
*/
chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(control_period_ms));
chMtxLock(&_mutex);
const uint64_t new_timestamp_hnsec = motor_rtctl_timestamp_hnsec();
const uint32_t dt_hnsec = new_timestamp_hnsec - timestamp_hnsec;
const float dt = dt_hnsec / (float)HNSEC_PER_SEC;
timestamp_hnsec = new_timestamp_hnsec;
assert(dt > 0);
update_filters(dt);
update_setpoint_ttl(dt_hnsec / HNSEC_PER_MSEC);
update_control(comm_period, dt);
poll_beep();
chMtxUnlock();
watchdog_reset(_watchdog_id);
}
assert_always(0);
return 0;
}
void BaseThread::SetPriority(tprio_t newprio) {
chThdSetPriority(newprio);
}
