/** * Executed by event dispatcher callback to reset INS before resetting OP */ static void resetTask(UAVObjEvent * ev) { PIOS_LED_Toggle(LED1); #if (PIOS_LED_NUM > 1) PIOS_LED_Toggle(LED2); #endif if((portTickType) (xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) { lastResetSysTime = xTaskGetTickCount(); data.BoardType=0xFF; data.ArmReset=1; data.crc=reset_count; /* Must change a value for this to get to INS */ FirmwareIAPObjSet(&data); ++reset_count; if(reset_count>3) { PIOS_SYS_Reset(); } } }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { /* create all modules thread */ MODULE_TASKCREATE_ALL; if (PIOS_heap_malloc_failed_p()) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); #if (defined(COPTERCONTROL) || defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX)) // Run this initially to make sure the configuration is checked configuration_check(); // Whenever the configuration changes, make sure it is safe to fly if (StabilizationSettingsHandle()) StabilizationSettingsConnectCallback(configurationUpdatedCb); if (SystemSettingsHandle()) SystemSettingsConnectCallback(configurationUpdatedCb); if (ManualControlSettingsHandle()) ManualControlSettingsConnectCallback(configurationUpdatedCb); if (FlightStatusHandle()) FlightStatusConnectCallback(configurationUpdatedCb); #endif #if (defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX)) if (StateEstimationHandle()) StateEstimationConnectCallback(configurationUpdatedCb); #endif // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(WDG_STATS_DIAGNOSTICS) updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF); #endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set if (indicateError()) { #if defined (PIOS_LED_ALARM) PIOS_LED_On(PIOS_LED_ALARM); #endif /* PIOS_LED_ALARM */ } else { #if defined (PIOS_LED_ALARM) PIOS_LED_Off(PIOS_LED_ALARM); #endif /* PIOS_LED_ALARM */ } FlightStatusData flightStatus; FlightStatusGet(&flightStatus); UAVObjEvent ev; int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? SYSTEM_UPDATE_PERIOD_MS / (LED_BLINK_RATE_HZ * 2) : SYSTEM_UPDATE_PERIOD_MS; if (PIOS_Queue_Receive(objectPersistenceQueue, &ev, delayTime) == true) { // If object persistence is updated call the callback objectUpdatedCb(&ev); } } }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { /* create all modules thread */ MODULE_TASKCREATE_ALL; if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(I2C_WDG_STATS_DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set #if defined (PIOS_LED_ALARM) if (AlarmsHasWarnings()) { PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); } #endif /* PIOS_LED_ALARM */ FlightStatusData flightStatus; FlightStatusGet(&flightStatus); UAVObjEvent ev; int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) : SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS; if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { // If object persistence is updated call the callback objectUpdatedCb(&ev); } } }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { portTickType lastSysTime; uint16_t prev_tx_count = 0; uint16_t prev_rx_count = 0; bool first_time = true; /* create all modules thread */ MODULE_TASKCREATE_ALL; if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } // Initialize vars idleCounter = 0; idleCounterClear = 0; lastSysTime = xTaskGetTickCount(); // Main system loop while (1) { // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ // Update the PipXstatus UAVO OPLinkStatusData oplinkStatus; uint32_t pairID; OPLinkStatusGet(&oplinkStatus); OPLinkSettingsPairIDGet(&pairID); // Get the other device stats. PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM); // Get the stats from the radio device struct rfm22b_stats radio_stats; PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); // Update the status oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); oplinkStatus.RxGood = radio_stats.rx_good; oplinkStatus.RxCorrected = radio_stats.rx_corrected; oplinkStatus.RxErrors = radio_stats.rx_error; oplinkStatus.RxMissed = radio_stats.rx_missed; oplinkStatus.RxFailure = radio_stats.rx_failure; oplinkStatus.TxDropped = radio_stats.tx_dropped; oplinkStatus.TxResent = radio_stats.tx_resent; oplinkStatus.TxFailure = radio_stats.tx_failure; oplinkStatus.Resets = radio_stats.resets; oplinkStatus.Timeouts = radio_stats.timeouts; oplinkStatus.RSSI = radio_stats.rssi; oplinkStatus.AFCCorrection = radio_stats.afc_correction; oplinkStatus.LinkQuality = radio_stats.link_quality; if (first_time) first_time = false; else { uint16_t tx_count = radio_stats.tx_byte_count; uint16_t rx_count = radio_stats.rx_byte_count; uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count); uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count); oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); prev_tx_count = tx_count; prev_rx_count = rx_count; } oplinkStatus.TXSeq = radio_stats.tx_seq; oplinkStatus.RXSeq = radio_stats.rx_seq; oplinkStatus.LinkState = radio_stats.link_state; if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) LINK_LED_ON; else LINK_LED_OFF; // Update the object OPLinkStatusSet(&oplinkStatus); // Wait until next period vTaskDelayUntil(&lastSysTime, MS2TICKS(SYSTEM_UPDATE_PERIOD_MS)); } }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { portTickType lastSysTime; /* create all modules thread */ MODULE_TASKCREATE_ALL; if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; lastSysTime = xTaskGetTickCount(); // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectCallback(&objectUpdatedCb); // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set #if defined (PIOS_LED_ALARM) if (AlarmsHasWarnings()) { PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); } #endif /* PIOS_LED_ALARM */ FlightStatusData flightStatus; FlightStatusGet(&flightStatus); // Wait until next period if(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) ); } else { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); } } }
void process_spi_request(void) { const struct pios_board_info * bdinfo = &pios_board_info_blob; bool msg_to_process = FALSE; PIOS_IRQ_Disable(); /* Figure out if we're in an interesting stable state */ switch (lfsm_get_state()) { case LFSM_STATE_USER_BUSY: msg_to_process = TRUE; break; case LFSM_STATE_INACTIVE: /* Queue up a receive buffer */ lfsm_user_set_rx_v0(&user_rx_v0); lfsm_user_done(); break; case LFSM_STATE_STOPPED: /* Get things going */ lfsm_set_link_proto_v0(&link_tx_v0, &link_rx_v0); break; default: /* Not a stable state */ break; } PIOS_IRQ_Enable(); if (!msg_to_process) { /* Nothing to do */ //PIOS_COM_SendFormattedString(PIOS_COM_AUX, "."); return; } if (user_rx_v0.tail.magic != OPAHRS_MSG_MAGIC_TAIL) { return; } switch (user_rx_v0.payload.user.t) { case OPAHRS_MSG_V0_REQ_FWUP_VERIFY: opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS); Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc(); lfsm_user_set_tx_v0(&user_tx_v0); boot_status = idle; PIOS_LED_Off(LED1); user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status; break; case OPAHRS_MSG_V0_REQ_RESET: PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.reset.reset_delay_in_ms); PIOS_SYS_Reset(); break; case OPAHRS_MSG_V0_REQ_VERSIONS: //PIOS_LED_On(LED1); opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_VERSIONS); user_tx_v0.payload.user.v.rsp.versions.bl_version = BOOTLOADER_VERSION; user_tx_v0.payload.user.v.rsp.versions.hw_version = (BOARD_TYPE << 8) | BOARD_REVISION; user_tx_v0.payload.user.v.rsp.versions.fw_crc = Fw_crc; lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_MEM_MAP: opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_MEM_MAP); user_tx_v0.payload.user.v.rsp.mem_map.density = bdinfo->hw_type; user_tx_v0.payload.user.v.rsp.mem_map.rw_flags = (BOARD_READABLE | (BOARD_WRITABLE << 1)); user_tx_v0.payload.user.v.rsp.mem_map.size_of_code_memory = bdinfo->fw_size; user_tx_v0.payload.user.v.rsp.mem_map.size_of_description = bdinfo->desc_size; user_tx_v0.payload.user.v.rsp.mem_map.start_of_user_code = bdinfo->fw_base; lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_SERIAL: opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_SERIAL); PIOS_SYS_SerialNumberGet( (char *) &(user_tx_v0.payload.user.v.rsp.serial.serial_bcd)); lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_FWUP_STATUS: opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS); user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status; lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_FWUP_DATA: PIOS_LED_On(LED1); opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS); if (!(user_rx_v0.payload.user.v.req.fwup_data.adress < bdinfo->fw_base)) { for (uint8_t x = 0; x < user_rx_v0.payload.user.v.req.fwup_data.size; ++x) { if (FLASH_ProgramWord( (user_rx_v0.payload.user.v.req.fwup_data.adress + ((uint32_t)(x * 4))), user_rx_v0.payload.user.v.req.fwup_data.data[x]) != FLASH_COMPLETE) { boot_status = write_error; break; } } } else { boot_status = outside_dev_capabilities; } PIOS_LED_Off(LED1); user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status; lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_FWDN_DATA: opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWDN_DATA); uint32_t adr = user_rx_v0.payload.user.v.req.fwdn_data.adress; for (uint8_t x = 0; x < 4; ++x) { user_tx_v0.payload.user.v.rsp.fw_dn.data[x] = *PIOS_BL_HELPER_FLASH_If_Read(adr + x); } lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_FWUP_START: FLASH_Unlock(); opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS); user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status; lfsm_user_set_tx_v0(&user_tx_v0); PIOS_LED_On(LED1); if (PIOS_BL_HELPER_FLASH_Start() == TRUE) { boot_status = started; PIOS_LED_Off(LED1); } else { boot_status = start_failed; break; } break; case OPAHRS_MSG_V0_REQ_BOOT: PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.boot.boot_delay_in_ms); FLASH_Lock(); jump_to_app(); break; default: break; } /* Finished processing the received message, requeue it */ lfsm_user_set_rx_v0(&user_rx_v0); lfsm_user_done(); return; }