void slaveLoop()
{
watchdogReset();
uint32_t now = micros();
bool needHop=false;
switch (state) {
case 0: // waiting for packet
if (RF_Mode == Received) {
Green_LED_ON;
Red_LED_OFF;
// got packet
lastReceived = now;
lostpkts=0;
linkQuality = (linkQuality << 1) | 1;
RF_Mode = Receive;
spiSendAddress(0x7f); // Send the package read command
for (int16_t i = 0; i < bind_data.packetSize; i++) {
rx_buf[i] = spiReadData();
}
uint8_t payloadBytes = (rx_buf[0] & 0x3f);
if (payloadBytes > (bind_data.packetSize - 1)) {
// INVALID DATA
payloadBytes = 0;
}
// Check if this is a new packet from master and not a resent one
if ((rx_buf[0] ^ tx_buf[0]) & MASTER_SEQ) {
tx_buf[0] ^= MASTER_SEQ;
if (payloadBytes) {
// DATA FRAME
if (bind_data.flags & PACKET_MODE) {
serialWrite(0xf0);
serialWrite(payloadBytes);
CRCtx = 0;
}
for (uint8_t i=0; i < payloadBytes; i++) {
serialWrite(rx_buf[1 + i]);
if ((bind_data.flags & PACKET_MODE) && (bind_data.flags & PACKETCRC_MODE)) {
CRC16_add(&CRCtx, rx_buf[1 + i]);
}
}
if ((bind_data.flags & PACKET_MODE) && (bind_data.flags & PACKETCRC_MODE)) {
serialWrite(CRCtx >> 8);
serialWrite(CRCtx & 0xff);
}
}
}
static bool sendAndWait(uint8_t reqType, uint8_t resType) {
outMsg.type = reqType;
// outer loop, retries
for (uint8_t i = 0; i < MAX_RESEND; i++) {
sendWrite(outMsg);
// loop 20 times, wait time 0.1s if no/wrong data => 2s
for (uint8_t j = 0; j < 20; j++) {
// loop 100 times, wait 1ms if no/wrong data => 0.1s
for (uint8_t k = 0; k < 100; k++) {
watchdogReset();
// Tx FIFO data available? (we don't care about the pipe here)
if (available(NULL)) {
// read message from FIFO, skip if size = 0
if (readMessage(inMsg.array) > 0) {
// protocol compatible? if not ignore msg
if ((mGetVersion(inMsg) != PROTOCOL_VERSION)) {
continue;
}
// msg for us?
if (inMsg.destination == nc.nodeId) {
// internal command: find parent
if ((mGetCommand(inMsg) == C_INTERNAL) && (inMsg.type == I_FIND_PARENT_RESPONSE)) {
// static parent found?
if (configuredParentID == inMsg.sender) {
configuredParentFound = true;
}
if ( ((inMsg.bValue < nc.distance - 1) && ( !configuredParentFound) ) || (configuredParentID == inMsg.sender)) {
// got new routing info, update settings
nc.distance = inMsg.bValue + 1;
nc.parentNodeId = inMsg.sender;
}
}
// did we receive expected reply?
if ((mGetCommand(inMsg) == mGetCommand(outMsg)) && (inMsg.type == resType)) {
return true;
}
}
}
} else {
// wait 1ms if no data available
_delay_ms(1);
}
}
}
}
return false;
}
void vApplicationIdleHook( void )
{
static uint32_t tickOfLatestWatchdogReset = M2T(0);
portTickType tickCount = xTaskGetTickCount();
if (tickCount - tickOfLatestWatchdogReset > M2T(WATCHDOG_RESET_PERIOD_MS))
{
tickOfLatestWatchdogReset = tickCount;
watchdogReset();
}
// Enter sleep mode. Does not work when debugging chip with SWD.
// Currently saves about 20mA STM32F405 current consumption (~30%).
#ifndef DEBUG
{ __asm volatile ("wfi"); }
#endif
}
uint8_t getch(void) {
uint8_t ch;
watchdogReset();
PORTD = 0; // setup PORTD for input
DDRD = 0; // input w/ tristate
while((PINC & C1_RXF)); // wait for data
PORTC |= C2_RD;
ch = PIND;
PORTC &= ~(C2_RD);
return ch;
// while(!(UCSR0A & _BV(RXC0)));
// ch = UDR0;
return ch;
}
uint8_t getch(void)
{
uint8_t ch;
while(!(UCSR0A & _BV(RXC0)));
if(!(UCSR0A & _BV(FE0))) {
/*
* A Framing Error indicates (probably) that something is talking
* to us at the wrong bit rate. Assume that this is because it
* expects to be talking to the application, and DON'T reset the
* watchdog. This should cause the bootloader to abort and run
* the application "soon", if it keeps happening. (Note that we
* don't care that an invalid char is returned...)
*/
watchdogReset();
}
ch = UDR0;
return ch;
}
int main(void)
{
halInit();
chSysInit();
sdStart(&STDOUT_SD, NULL);
watchdogInit(WATCHDOG_TIMEOUT_MS);
const int wdid = watchdogStart();
restoreConfig();
magnetInit();
consoleInit();
int res = canasctlInit();
if (res)
{
TRACE("init", "CANAS NOT INITED (%d), FIX CONFIG AND RESTART", res);
while (1)
{
ledOn();
watchdogReset(wdid);
chThdSleepMilliseconds(100);
}
}
#if RELEASE
TRACE("init", "debug port will be disabled %d sec later", DEBUG_PORT_DISABLE_DEADLINE_SEC);
#endif
bool debug_port_disabled = false;
while (1)
{
const bool
feedback = magnetReadFeedback(),
requested = magnetGetRequestedState();
ledOn();
if (feedback != requested)
{
chThdSleepMilliseconds(70);
ledOff();
chThdSleepMilliseconds(70);
}
else if (feedback)
{
chThdSleepMilliseconds(500);
ledOff();
chThdSleepMilliseconds(500);
}
else
{
chThdSleepMilliseconds(70);
ledOff();
chThdSleepMilliseconds(930);
}
if (!debug_port_disabled)
{
if (sysTimestampMicros() / 1000000 > DEBUG_PORT_DISABLE_DEADLINE_SEC)
{
#if RELEASE
debugPortDisable();
#endif
debug_port_disabled = true;
}
}
watchdogReset(wdid);
}
return 0;
}
// main start
int main(void) {
asm volatile ("clr __zero_reg__");
// reset MCU status register
MCUSR = 0;
// enable watchdog to avoid deadlock
watchdogConfig(WATCHDOG_8S);
// initialize SPI
SPIinit();
// initialize RF module
RFinit();
// Read node config from EEPROM, i.e. nodeId, parent nodeId, distance
eeprom_read_block((void*)&nc, (void*)EEPROM_NODE_ID_ADDRESS, sizeof(struct NodeConfig));
// Read firmware config from EEPROM, i.e. type, version, CRC, blocks
eeprom_read_block((void*)&fc, (void*)EEPROM_FIRMWARE_TYPE_ADDRESS, sizeof(NodeFirmwareConfig));
// find nearest node during reboot: invalidate parent node settings, since we have to re-discover them for every single reboot
configuredParentID = nc.parentNodeId;
// nc.parentNodeId = 0xFF;
nc.distance = 0xFF;
// prepare for I_FIND_PARENTS
outMsg.sender = nc.nodeId;
outMsg.last = nc.nodeId;
outMsg.sensor = 0xFF;
outMsg.destination = BROADCAST_ADDRESS;
// set header
mSetVersion(outMsg, PROTOCOL_VERSION);
mSetLength(outMsg, 0);
mSetCommand(outMsg, C_INTERNAL);
mSetAck(outMsg,false);
mSetPayloadType(outMsg, P_STRING);
// set reading & writing pipe address
setAddress(nc.nodeId);
// network up? get neighbors, else startup
if (!sendAndWait(I_FIND_PARENT, I_FIND_PARENT_RESPONSE)) {
startup();
}
// all messages to gateway
outMsg.destination = GATEWAY_ADDRESS;
// if no node id assigned, request new id
if (nc.nodeId == AUTO) {
// listen to broadcast
openReadingPipe(CURRENT_NODE_PIPE, TO_ADDR(BROADCAST_ADDRESS));
if (sendAndWait(I_ID_REQUEST, I_ID_RESPONSE)) {
// save id to eeprom
eeprom_update_byte((uint8_t*)EEPROM_NODE_ID_ADDRESS, atoi(inMsg.data));
}
// we could go on and set everything right here, but rebooting will take care of that - and saves some bytes :)
reboot();
}
// wuff
watchdogReset();
// prepare for FW config request
RequestFirmwareConfig *reqFWConfig = (RequestFirmwareConfig *)outMsg.data;
mSetLength(outMsg, sizeof(RequestFirmwareConfig));
mSetCommand(outMsg, C_STREAM);
mSetPayloadType(outMsg,P_CUSTOM);
// copy node settings to reqFWConfig
memcpy(reqFWConfig,&fc,sizeof(NodeFirmwareConfig));
// add bootloader information
reqFWConfig->BLVersion = MYSBOOTLOADER_VERSION;
// send node config and request FW config from controller
if (!sendAndWait(ST_FIRMWARE_CONFIG_REQUEST, ST_FIRMWARE_CONFIG_RESPONSE)) {
startup();
}
NodeFirmwareConfig *firmwareConfigResponse = (NodeFirmwareConfig *)inMsg.data;
// bootloader commands
if (firmwareConfigResponse->blocks == 0) {
// verify flag
if (firmwareConfigResponse->crc == 0xDA7A){
// cmd 0x01 clear eeprom
if(firmwareConfigResponse->bl_command == 0x01) {
for(uint16_t i = 0; i < EEPROM_SIZE; i++) eeprom_update_byte((uint8_t *)i,0xFF);
} else
// cmd 0x02 set id
if(firmwareConfigResponse->bl_command == 0x02) {
eeprom_update_byte((uint8_t*)EEPROM_NODE_ID_ADDRESS, (uint8_t)firmwareConfigResponse->bl_data);
}
}
// final step
reboot();
}
// compare with current node configuration, if equal startup
if (!memcmp(&fc,firmwareConfigResponse,sizeof(NodeFirmwareConfig))) {
startup();
}
// *********** from here on we will fetch new FW
// invalidate current CRC
fc.crc = 0xFFFF;
// write fetched type and version in case OTA fails (BL will reboot and re-request FW with stored settings)
eeprom_update_block(&fc, (void*)EEPROM_FIRMWARE_TYPE_ADDRESS,sizeof(NodeFirmwareConfig));
// copy new FW config
memcpy(&fc,firmwareConfigResponse,sizeof(NodeFirmwareConfig));
RequestFWBlock *firmwareRequest = (RequestFWBlock *)outMsg.data;
mSetLength(outMsg, sizeof(RequestFWBlock));
firmwareRequest->type = fc.type;
firmwareRequest->version = fc.version;
// request FW from controller, load FW counting backwards
uint16_t block = fc.blocks;
do {
firmwareRequest->block = block - 1;
// request FW block
if (!sendAndWait(ST_FIRMWARE_REQUEST, ST_FIRMWARE_RESPONSE)) {
reboot();
}
ReplyFWBlock *firmwareResponse = (ReplyFWBlock *)inMsg.data;
// did we receive requested block?
if (!memcmp(firmwareRequest,firmwareResponse,sizeof(RequestFWBlock))) {
// calculate page offset
uint8_t offset = ((block - 1) * FIRMWARE_BLOCK_SIZE) % SPM_PAGESIZE;
// write to buffer
memcpy(progBuf + offset, firmwareResponse->data, FIRMWARE_BLOCK_SIZE);
// program if page full
if (offset == 0) {
programPage(((block - 1) * FIRMWARE_BLOCK_SIZE), progBuf);
}
block--;
}
} while (block);
// wuff
watchdogReset();
// all blocks transmitted, calc CRC and write to eeprom if valid
if (IsFirmwareValid()) {
// if FW is valid, write settings to eeprom
eeprom_update_block(&fc, (void*)EEPROM_FIRMWARE_TYPE_ADDRESS, sizeof(NodeFirmwareConfig));
}
// final step
reboot();
}
static void control_thread(void* arg)
{
(void)arg;
chRegSetThreadName("motor");
event_listener_t 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 != chThdGetPriorityX()) {
const tprio_t old = chThdSetPriority(desired_thread_priority);
printf("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(&_mutex);
watchdogReset(_watchdog_id);
}
abort();
}
