static void check_and_update_heartbeat_watchdog() { ss_id_e i; bool all_alive; subsystem_t *ss; all_alive = true; if (watchdog_has_started_a_new_period()) { // log_report(LOG_SS_PLATFORM, "PLATFORM: All subsystem's heartbeats cleared\n"); for (i=0; i < ARRAY_COUNT(subsystems); i++) { if (IS_SS_VALID(i)) { subsystems[i]->state->current_heartbeats = 0; } } } else { for (i=0; i < ARRAY_COUNT(subsystems); i++) { if (!IS_SS_VALID(i)) continue; ss = subsystems[i]; if ((ss->state->current_heartbeats & ss->state->expected_heartbeats) != ss->state->expected_heartbeats) { all_alive = false; PLATFORM_save_boot_reason(ss->config->name); // log_report_fmt(LOG_SS_PLATFORM, "PLARFORM: Subsystem %s missed a heartbeat (%02x != %02x)\n", // ss->config->name, // ss->state->current_heartbeats, // ss->state->expected_heartbeats); } } if (all_alive) { // log_report(LOG_SS_PLATFORM, "PLATFORM: All subsystems are alive!\n"); watchdog_restart_counter(); POWER_watchdog_restart_counter(); led_toggle(1); /* triggers the Alive LED */ PLATFORM_save_boot_reason_unexpected(); } } }
int main(void) { HAL_Init(); SystemClock_Config(); RCC_Config(); GPIO_Config(); UART_Config(); if (SysTick_Config(SystemCoreClock / 1000)) { error_handler(); } log_init(LOGLEVEL_INFO); log_info("PROJECT STERTED"); while (1) { led_toggle(LED_BLUE); log_info("Hello! :)"); HAL_Delay(DELAY); } }
void __fatal_error(const char *msg) { for (volatile uint delay = 0; delay < 10000000; delay++) { } led_state(1, 1); led_state(2, 1); led_state(3, 1); led_state(4, 1); stdout_tx_strn("\nFATAL ERROR:\n", 14); stdout_tx_strn(msg, strlen(msg)); #if 0 && MICROPY_HW_HAS_LCD lcd_print_strn("\nFATAL ERROR:\n", 14); lcd_print_strn(msg, strlen(msg)); #endif for (uint i = 0;;) { led_toggle(((i++) & 3) + 1); for (volatile uint delay = 0; delay < 10000000; delay++) { } if (i >= 16) { // to conserve power __WFI(); } } }
// EXTI // for USRSW on A13 // for cc3000 on A14 void EXTI15_10_IRQHandler(void) { // work out if it's A13 that had the interrupt if (EXTI_GetITStatus(EXTI_Line13) != RESET) { // this is used just to wake the device EXTI_ClearITPendingBit(EXTI_Line13); } // work out if it's A14 that had the interrupt if (EXTI_GetITStatus(EXTI_Line14) != RESET) { led_toggle(PYB_LED_G2); extern void SpiIntGPIOHandler(void); extern uint32_t exti14_enabled; extern uint32_t exti14_missed; //printf("-> EXTI14 en=%lu miss=%lu\n", exti14_enabled, exti14_missed); if (exti14_enabled) { exti14_missed = 0; SpiIntGPIOHandler(); // CC3000 interrupt } else { exti14_missed = 1; } EXTI_ClearITPendingBit(EXTI_Line14); //printf("<- EXTI14 done\n"); } }
int main(void) { setup(); printf("%s\r\n", "STARTING LOOP"); float encoder_master_count = 0; float previous_degs = 0; float current_degs = 0; uint16_t current_ticks = 0; uint16_t previous_ticks = spi_read_ticks(); while (1) { if (timer_flag(&timer2)) { // Blink green light to show normal operation. timer_lower(&timer2); led_toggle(&led2); printf("ddegs %3f, dd %d\r\n", delta_degs, drive_direction); } if (timer_flag(&timer3)) { timer_lower(&timer3); delta_degs = current_degs - previous_degs; current_duty_cycle = damper(delta_degs); previous_degs = current_degs; } if (!sw_read(&sw2)) { // If switch 2 is pressed, the UART output terminal is cleared. printf("%s", clear); } ServiceUSB(); current_ticks = spi_read_ticks(); encoder_master_count = encoder_counter(current_ticks, previous_ticks, encoder_master_count); current_degs = count_to_deg(encoder_master_count); previous_ticks = current_ticks; } }
void led_net_callback(config_section_t * conf_sect, const msg_lvl2_t * net_msg) { int rc; if(net_msg != NULL) { pmd_net_led_data_t pmd_led_data; rc = pmd_net_led_read_data(&(net_msg->data), &pmd_led_data); if(rc == 0) { switch(pmd_led_data.operation) { case PMD_NET_LED_ON: led_on(conf_sect); break; case PMD_NET_LED_OFF: led_off(conf_sect); break; case PMD_NET_LED_TOGGLE: led_toggle(conf_sect); break; } } } }
void led_twinkle( uint8 id ,uint16 interval ) { switch(id) { case LED_GREEN: led_toggle(LED_GREEN); hal_delay( interval ); led_toggle(LED_GREEN); break; case LED_RED: led_toggle(LED_RED); hal_delay( interval ); led_toggle(LED_RED); break; case LED_YELLOW: led_toggle(LED_YELLOW); hal_delay( interval ); led_toggle(LED_YELLOW); break; } }
/********************************* main entry point *********************************/ int main ( void ) { // Init the basic hardware InitCnsts(); InitHardware(); // Start the main 1Khz timer and PWM timer InitTimer1(); InitTimer2(); InitPWM(); // Initialize A2D, InitA2D(); UART1_Init(XBEE_SPEED); // for communication and control signals UART2_Init(LOGGING_RC_SPEED); // for spektrum RC satellite receiver // Wait for a bit before doing rate gyro bias calibration // TODO: test this length of wait uint16_t i=0;for(i=0;i<60000;i++){Nop();} // turn on the leds until the bias calibration is complete led_on(LED_RED); led_on(LED_GREEN); // Initialize the AHRS AHRS_init(); // Initialize the Controller variables Controller_Init(); // MAIN CONTROL LOOP: Loop forever while (1) { // Gyro propagation if(loop.GyroProp){ loop.GyroProp = 0; // Call gyro propagation AHRS_GyroProp(); } // Attitude control if(loop.AttCtl){ loop.AttCtl = 0; // Call attitude control Controller_Update(); } // Accelerometer correction if( loop.ReadAccMag ){ loop.ReadAccMag = 0; AHRS_AccMagCorrect( ); } // Send data over modem - runs at ~20Hz if(loop.SendSerial){ loop.SendSerial = 0; // Send debug packet UART1_SendAHRSpacket(); } // Process Spektrum RC data if(loop.ProcessSpektrum){ loop.ProcessSpektrum = 0; UART2_ProcessSpektrumData(); } // Read data from UART RX buffers - 500 Hz if(loop.ReadSerial){ loop.ReadSerial = 0; // Read serial data //UART2_FlushRX_Spektrum(); } // Toggle Red LED at 1Hz if(loop.ToggleLED){ loop.ToggleLED = 0; // Toggle LED led_toggle(LED_RED); } } // End while(1) }
// Timer callback static void timer_handler (void* p_context) { led_toggle(LED); }
static THD_FUNCTION(rx_thread, arg) { (void)arg; chRegSetThreadName("CC2520 RX"); for(;;) { if (basicRfPacketIsReady()) { static uint8_t buf[130]; unsigned int len = 0; unsigned int ind = 0; len = basicRfReceive(buf, 130, NULL); MOTE_PACKET packet = buf[0]; led_toggle(LED_RED); switch (packet) { case MOTE_PACKET_FILL_RX_BUFFER: memcpy(rx_buffer + buf[1], buf + 2, len - 2); break; case MOTE_PACKET_FILL_RX_BUFFER_LONG: { int rxbuf_ind = (unsigned int)buf[1] << 8; rxbuf_ind |= buf[2]; if (rxbuf_ind < RX_BUFFER_SIZE) { memcpy(rx_buffer + rxbuf_ind, buf + 3, len - 3); } } break; case MOTE_PACKET_PROCESS_RX_BUFFER: { ind = 1; int rxbuf_len = (unsigned int)buf[ind++] << 8; rxbuf_len |= (unsigned int)buf[ind++]; if (rxbuf_len > RX_BUFFER_SIZE) { break; } uint8_t crc_high = buf[ind++]; uint8_t crc_low = buf[ind++]; memcpy(rx_buffer + rxbuf_len - (len - ind), buf + ind, len - ind); if (crc16(rx_buffer, rxbuf_len) == ((unsigned short) crc_high << 8 | (unsigned short) crc_low)) { comm_usb_send_packet(rx_buffer, rxbuf_len); } } break; case MOTE_PACKET_PROCESS_SHORT_BUFFER: comm_usb_send_packet(buf + 1, len - 1); break; default: break; } } chThdSleepMicroseconds(100); } }
int preflight_check_main(int argc, char *argv[]) { bool fail_on_error = false; if (argc > 1 && !strcmp(argv[1], "--help")) { warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error"); exit(1); } if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) { fail_on_error = true; } bool system_ok = true; int fd; /* open text message output path */ int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; /* give the system some time to sample the sensors in the background */ usleep(150000); /* ---- MAG ---- */ close(fd); fd = open(MAG_DEVICE_PATH, 0); if (fd < 0) { warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG"); system_ok = false; goto system_eval; } ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); system_ok = false; goto system_eval; } /* ---- ACCEL ---- */ close(fd); fd = open(ACCEL_DEVICE_PATH, 0); ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { warnx("accel self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); system_ok = false; goto system_eval; } /* ---- GYRO ---- */ close(fd); fd = open(GYRO_DEVICE_PATH, 0); ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret != OK) { warnx("gyro self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); system_ok = false; goto system_eval; } /* ---- BARO ---- */ close(fd); fd = open(BARO_DEVICE_PATH, 0); /* ---- RC CALIBRATION ---- */ param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, _parameter_handles_rev, _parameter_handles_dz; float param_min, param_max, param_trim, param_rev, param_dz; bool rc_ok = true; char nbuf[20]; for (int i = 0; i < 12; i++) { /* should the channel be enabled? */ uint8_t count = 0; /* min values */ sprintf(nbuf, "RC%d_MIN", i + 1); _parameter_handles_min = param_find(nbuf); param_get(_parameter_handles_min, ¶m_min); /* trim values */ sprintf(nbuf, "RC%d_TRIM", i + 1); _parameter_handles_trim = param_find(nbuf); param_get(_parameter_handles_trim, ¶m_trim); /* max values */ sprintf(nbuf, "RC%d_MAX", i + 1); _parameter_handles_max = param_find(nbuf); param_get(_parameter_handles_max, ¶m_max); /* channel reverse */ sprintf(nbuf, "RC%d_REV", i + 1); _parameter_handles_rev = param_find(nbuf); param_get(_parameter_handles_rev, ¶m_rev); /* channel deadzone */ sprintf(nbuf, "RC%d_DZ", i + 1); _parameter_handles_dz = param_find(nbuf); param_get(_parameter_handles_dz, ¶m_dz); /* assert min..center..max ordering */ if (param_min < 500) { count++; mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_max > 2500) { count++; mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim < param_min) { count++; mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim > param_max) { count++; mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); /* give system time to flush error message in case there are more */ usleep(100000); } /* assert deadzone is sane */ if (param_dz > 500) { mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); /* give system time to flush error message in case there are more */ usleep(100000); count++; } /* XXX needs inspection of all the _MAP params */ // if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); // /* give system time to flush error message in case there are more */ // usleep(100000); // count++; // } /* sanity checks pass, enable channel */ if (count) { mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); usleep(100000); rc_ok = false; } } /* require RC ok to keep system_ok */ system_ok &= rc_ok; system_eval: if (system_ok) { /* all good, exit silently */ exit(0); } else { fflush(stdout); int buzzer = open("/dev/tone_alarm", O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); /* flip blue led into alternating amber */ led_off(leds, LED_BLUE); led_off(leds, LED_AMBER); led_toggle(leds, LED_BLUE); /* display and sound error */ for (int i = 0; i < 150; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); if (i % 10 == 0) { ioctl(buzzer, TONE_SET_ALARM, 4); } else if (i % 5 == 0) { ioctl(buzzer, TONE_SET_ALARM, 2); } usleep(100000); } /* stop alarm */ ioctl(buzzer, TONE_SET_ALARM, 0); /* switch on leds */ led_on(leds, LED_BLUE); led_on(leds, LED_AMBER); if (fail_on_error) { /* exit with error message */ exit(1); } else { /* do not emit an error code to make sure system still boots */ exit(0); } } }
void updateProgram(void) { int clear = 0; int cmdByte, i, temp; FLASH_EraseResult result; uint32_t addr, maxAddr = 0; uint16_t endSector = 0xFFFF; _ee_getReserved(_AI_EE_RES_ADDR_MAX_SECTOR, &endSector); if (endSector > APPLICATION_END_SECTOR || !IS_FLASH_SECTOR(endSector)) endSector = APPLICATION_END_SECTOR; lcd_clear(); lcd_printf("Aithon Board\nProgramming..."); // Unlock the Flash Program Erase controller FLASH_If_Init(); while (TRUE) { led_toggle(0); cmdByte = getByte(); debugPrintCmd(cmdByte); switch (cmdByte) { case SYNC: // sync flushInterface(); sendResponse(SYNC, ACK); break; case ERASE_FLASH_START: if (FLASH_If_Erase_Start() == FLASH_ERASE_IN_PROGRESS) sendResponse(ERASE_FLASH_START, ACK); else sendResponse(ERASE_FLASH_START, NACK); break; case ERASE_FLASH_STATUS: result = FLASH_If_Erase_Status(endSector); if (result == FLASH_ERASE_COMPLETE) sendResponse(ERASE_FLASH_STATUS, ACK); else if (result == FLASH_ERASE_IN_PROGRESS) sendResponse(ERASE_FLASH_STATUS, BUSY); else sendResponse(ERASE_FLASH_STATUS, NACK); break; case SET_ADDR: // Read in the address, MSB first. addr = 0; for (i = 0; i < 4; i++) { if ((temp = getByte()) == Q_TIMEOUT) break; addr |= (((uint8_t) temp) & 0xFF) << (i * 8); } // Check for errors. if (temp == Q_TIMEOUT) sendResponse(SET_ADDR, NACK); else { sendResponse(SET_ADDR, ACK); // We'll get relative addresses, so add the start address. addr += APPLICATION_START_ADDRESS; } break; case CHECK_ADDR: // Get the checksum temp = getByte(); if (temp == Q_TIMEOUT) sendResponse(CHECK_ADDR, NACK); else { // Subtract the start address before calculating the checksum addr -= APPLICATION_START_ADDRESS; if (temp == calcChecksum((uint8_t *)&addr, 4)) sendResponse(CHECK_ADDR, ACK); else sendResponse(CHECK_ADDR, NACK); addr += APPLICATION_START_ADDRESS; } break; case FILL_BUFFER: for (i = 0; i < PACKET_LEN; i++) { if ((temp = getByte()) == Q_TIMEOUT) break; _buffer[i] = (uint8_t) (temp & 0xFF); } if (temp == Q_TIMEOUT) sendResponse(FILL_BUFFER, NACK); else sendResponse(FILL_BUFFER, ACK); break; case CHECK_BUFFER: // Get the checksum temp = getByte(); if (temp != Q_TIMEOUT && temp == calcChecksum(_buffer, PACKET_LEN)) sendResponse(CHECK_BUFFER, ACK); else sendResponse(CHECK_BUFFER, NACK); break; case COMMIT_BUFFER: maxAddr = addr + PACKET_LEN - 1; if (FLASH_If_Write((__IO uint32_t *)&addr, (uint32_t *)_buffer, PACKET_LEN/4)) sendResponse(COMMIT_BUFFER, NACK); else sendResponse(COMMIT_BUFFER, ACK); break; case START_PROGRAM: sendResponse(START_PROGRAM, ACK); flushInterface(); _ee_putReserved(_AI_EE_RES_ADDR_MAX_SECTOR, FLASH_Addr_To_Sector(maxAddr)); delayS(1); startProgram(); // ...should never get here return; case Q_TIMEOUT: default: if (clear == 0) { lcd_clear(); clear = 1; } lcd_printf ("0%x ", cmdByte); break; } } }
int preflight_check_main(int argc, char *argv[]) { bool fail_on_error = false; if (argc > 1 && !strcmp(argv[1], "--help")) { warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error"); exit(1); } if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) { fail_on_error = true; } bool system_ok = true; int fd; /* open text message output path */ int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; /* give the system some time to sample the sensors in the background */ usleep(150000); /* ---- MAG ---- */ close(fd); fd = open(MAG_DEVICE_PATH, 0); if (fd < 0) { warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG"); system_ok = false; goto system_eval; } ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); system_ok = false; goto system_eval; } /* ---- ACCEL ---- */ close(fd); fd = open(ACCEL_DEVICE_PATH, 0); ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { warnx("accel self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); system_ok = false; goto system_eval; } /* ---- GYRO ---- */ close(fd); fd = open(GYRO_DEVICE_PATH, 0); ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret != OK) { warnx("gyro self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); system_ok = false; goto system_eval; } /* ---- BARO ---- */ close(fd); fd = open(BARO_DEVICE_PATH, 0); close(fd); /* ---- RC CALIBRATION ---- */ bool rc_ok = (OK == rc_calibration_check()); /* warn */ if (!rc_ok) warnx("rc calibration test failed"); /* require RC ok to keep system_ok */ system_ok &= rc_ok; system_eval: if (system_ok) { /* all good, exit silently */ exit(0); } else { fflush(stdout); warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); fflush(stderr); int buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { close(buzzer); errx(1, "failed to open leds, aborting"); } /* flip blue led into alternating amber */ led_off(leds, LED_BLUE); led_off(leds, LED_AMBER); led_toggle(leds, LED_BLUE); /* display and sound error */ for (int i = 0; i < 50; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); if (i % 10 == 0) { ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } else if (i % 5 == 0) { ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); } usleep(100000); } /* stop alarm */ ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); /* switch on leds */ led_on(leds, LED_BLUE); led_on(leds, LED_AMBER); if (fail_on_error) { /* exit with error message */ exit(1); } else { /* do not emit an error code to make sure system still boots */ exit(0); } } }
static void CLI_blink_switch(servo_usb_control_context_t *ctx, int on){ (void) ctx; led_toggle(); blink_switch(on); }
/** * @brief Receive communication packets and handle them * * This function decodes packets on the protocol level and also handles * their value by calling the appropriate functions. */ void communication_receive(void) { mavlink_message_t msg; mavlink_status_t status = { 0 }; status.packet_rx_drop_count = 0; // COMMUNICATION WITH ONBOARD COMPUTER while (uart0_char_available()) { uint8_t c = uart0_get_char(); if (global_data.state.uart0mode == UART_MODE_MAVLINK) { // Try to get a new message if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // Handle message handle_mavlink_message(MAVLINK_COMM_0, &msg); } } else if (global_data.state.uart0mode == UART_MODE_BYTE_FORWARD) { uart1_transmit(c); } // And get the next one } // Update global packet drops counter global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count; global_data.comm.uart0_rx_success_count += status.packet_rx_success_count; status.packet_rx_drop_count = 0; // COMMUNICATION WITH EXTERNAL COMPUTER while (uart1_char_available()) { uint8_t c = uart1_get_char(); // Check if this link is used for MAVLink or GPS if (global_data.state.uart1mode == UART_MODE_MAVLINK) { //uart0_transmit((unsigned char)c); // Try to get a new message if (mavlink_parse_char(MAVLINK_COMM_1, c, &msg, &status)) { // Handle message handle_mavlink_message(MAVLINK_COMM_1, &msg); } } else if (global_data.state.uart1mode == UART_MODE_GPS) { if (global_data.state.gps_mode == 10) { static uint8_t gps_i = 0; static char gps_chars[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; if (c == '$' || gps_i == MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN - 1) { gps_i = 0; char gps_chars_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; strncpy(gps_chars_buf, gps_chars, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); debug_message_buffer(gps_chars_buf); } gps_chars[gps_i++] = c; } if (gps_parse(c)) { // New GPS data received //debug_message_buffer("RECEIVED NEW GPS DATA"); parse_gps_msg(); if (gps_lat == 0) { global_data.state.gps_ok = 0; //debug_message_buffer("GPS Signal Lost"); } else { global_data.state.gps_ok = 1; mavlink_msg_gps_raw_send( global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_time(), gps_mode, gps_lat / 1e7f, gps_lon / 1e7f, gps_alt / 100.0f, 0.0f, 0.0f, gps_gspeed / 100.0f, gps_course / 10.0f); } // // Output satellite info // for (int i = 0; i < gps_nb_channels; i++) // { // mavlink_msg_gps_status_send(global_data.param[PARAM_SEND_DEBUGCHAN], gps_numSV, gps_svinfos[i].svid, gps_satellite_used(gps_svinfos[i].qi), gps_svinfos[i].elev, ((gps_svinfos[i].azim/360.0f)*255.0f), gps_svinfos[i].cno); // } } } else if (global_data.state.uart1mode == UART_MODE_BYTE_FORWARD) { uart0_transmit(c); led_toggle(LED_YELLOW); } // And get the next one } // Update global packet drops counter global_data.comm.uart0_rx_drop_count += status.packet_rx_drop_count; global_data.comm.uart0_rx_success_count += status.packet_rx_success_count; status.packet_rx_drop_count = 0; }
void wait_for_clock() { led_toggle(); while (gpio_pin_read(GPIO_PIN23) == 0) {} while (gpio_pin_read(GPIO_PIN23) == 1) {} }
/**@breif Function for checking event flags and handling them. */ void check_and_handle_asc_flags(void) { uint32_t asc_flags = ascs_events_get(); do { if (asc_flags & EVENT_ASC_STATE_CHANGED) { ascs_event_clear(EVENT_ASC_STATE_CHANGED); //Determine which state changed and react ascs_states_t state = ascs_state_get(); switch(state) { case ASSIGNED: { led_on(CONNECTED_LED); break; } case SEARCHING: //Intentional fallthrough case REQUESTING: //Intentional fallthrough case WAITING: //Intentional fallthrough case CONFIRMING: //Intentional fallthrough case OFF: //Intentional fallthrough default: { led_off(CONNECTED_LED); break; } } } if (asc_flags & EVENT_ASC_DEVICE_IN_WRONG_STATE) { ascs_event_clear(EVENT_ASC_DEVICE_IN_WRONG_STATE); //Do nothing for(;;) { led_toggle(LED_ERROR_0); led_toggle(LED_ERROR_1); led_toggle(LED_ERROR_2); led_toggle(LED_ERROR_3); nrf_delay_ms(200); } } if (asc_flags & EVENT_ASC_LIGHT_STATE_CHANGED) { ascs_event_clear(EVENT_ASC_LIGHT_STATE_CHANGED); asc_slave_states_t light_state = ascs_light_state_get(); switch(light_state) { case SLAVE_STATE_ON: { led_on(LIGHT); break; } case SLAVE_STATE_OFF: { led_off(LIGHT); break; } default: { break; } } } if (asc_flags & EVENT_ASC_REQUEST_RECEIVED) { ascs_event_clear(EVENT_ASC_REQUEST_RECEIVED); uint8_t response_buffer[ANT_STANDARD_DATA_PAYLOAD_SIZE] = {0}; asc_request_data_t request = ascs_get_last_request(); //encode the response buffer with whatever page was requested switch(request.page_id_requested) { default: { break; } } //tell the slave to send the response ascs_send_response(response_buffer); } if (asc_flags & EVENT_ASC_DATA_TIMEOUT) { ascs_event_clear(EVENT_ASC_DATA_TIMEOUT); //Do nothing } asc_flags = ascs_events_get(); } while(asc_flags != 0); }
void tinymac_recvnode(void) { TiCc2420Adapter * cc; TiFrameRxTxInterface * rxtx;; TiTinyMAC * mac; TiTimerAdapter *timer; TiFrame * rxbuf; char * msg = "welcome to tinymac recv test..."; int len=0; target_init(); // flash the led to indicate the software is successfully running now. // led_open(); led_on( LED_ALL ); hal_delay( 500 ); led_off( LED_ALL ); led_on( LED_RED ); // initialize the runtime library for debugging input/output and assertion // hal_assert_report is defined in module "hal_assert" // //dbo_open( 38400 ); rtl_init( (void *)dbio_open(38400), (TiFunDebugIoPutChar)dbio_putchar, (TiFunDebugIoGetChar)dbio_getchar, hal_assert_report ); dbc_putchar( 0xF0 ); dbc_mem( msg, strlen(msg) ); cc = cc2420_construct( (void *)(&m_cc), sizeof(TiCc2420Adapter) ); mac = tinymac_construct((char *)(&m_tiny), sizeof(TiTinyMAC)); timer= timer_construct((char *)(&m_timer),sizeof(TiTimerAdapter)); #ifdef CONFIG_TSET_LISTENER // cc = cc2420_open( cc, 0, _aloha_listener, NULL, 0x00 ); cc = cc2420_open( cc, 0, tinymac_evolve, mac, 0x00 ); rxtx = cc2420_interface( cc, &m_rxtx ); mac = tinymac_open( mac, rxtx, CONFIG_CHANNEL, CONFIG_PANID, CONFIG_LOCAL_ADDRESS, timer, _tinymac_listener, NULL,0x00 ); #endif #ifndef CONFIG_TSET_LISTENER cc = cc2420_open( cc, 0, NULL, NULL, 0x00 ); rxtx = cc2420_interface( cc, &m_rxtx ); mac = tinymac_open( mac, rxtx, CONFIG_CHANNEL, CONFIG_PANID, CONFIG_LOCAL_ADDRESS, NULL, NULL,0x00 ); #endif cc2420_setchannel( cc, CONFIG_CHANNEL ); cc2420_setrxmode( cc ); // enable RX mode cc2420_setpanid( cc, CONFIG_PANID ); // network identifier, seems no use in sniffer mode cc2420_setshortaddress( cc, CONFIG_LOCAL_ADDRESS ); // in network address, seems no use in sniffer mode cc2420_enable_autoack( cc ); #ifdef CONFIG_TEST_ADDRESSRECOGNITION cc2420_enable_addrdecode( cc ); #else cc2420_disable_addrdecode( cc ); #endif #ifdef CONFIG_TEST_ACK cc2420_enable_autoack( cc ); #endif rxbuf = frame_open( (char*)(&m_rxbufmem), FRAME_HOPESIZE(MAX_IEEE802FRAME154_SIZE), 3, 20, 0 ); #ifdef CONFIG_TEST_ACK //fcf = OPF_DEF_FRAMECONTROL_DATA_ACK; #else //fcf = OPF_DEF_FRAMECONTROL_DATA_NOACK; #endif hal_enable_interrupts(); /* Wait for listener action. The listener function will be called by the TiCc2420Adapter * object when a frame arrives */ #ifdef CONFIG_TEST_LISTENER while (1) {} #endif /* Query the TiCc2420Adapter object if there's no listener */ #ifndef CONFIG_TEST_LISTENER while(1) { frame_reset( rxbuf, 3, 20, 0 ); len = tinymac_recv( mac, rxbuf, 0x00 ); if (len > 0) { dbc_putchar( 0xF3 ); dbc_mem( frame_startptr(rxbuf), frame_length(rxbuf) ); //frame_moveouter( rxbuf ); //_output_frame( rxbuf, NULL ); //frame_moveinner( rxbuf ); led_off( LED_RED ); /* warning: You shouldn't wait too long in the while loop, or else * you may encounter frame loss. However, the program should still * work properly even the delay time is an arbitrary value. No error * are allowed in this case. */ //hal_delay( 500 ); led_toggle( LED_RED ); //hal_delay( 500 ); } tinymac_evolve(mac, NULL ); } #endif frame_close( rxbuf ); tinymac_close( mac ); cc2420_close( cc ); }
void led_test( void ) { int i; target_init(); led_open(); led_on( LED_ALL ); hal_delay( 1000 ); /* testing each single LED */ led_off( LED_ALL ); i=0; while (i<3) { led_off(LED_RED) ; hal_delay(300); led_on(LED_RED) ; hal_delay(300); i++; } led_off( LED_RED ); i=0; while (i<3) { led_off(LED_GREEN) ; hal_delay(300); led_on(LED_GREEN) ; hal_delay(300); i++; } led_off( LED_GREEN ); i=0; while (i<3) { led_off(LED_YELLOW) ; hal_delay(300); led_on(LED_YELLOW) ; hal_delay(300); i++; } led_off( LED_YELLOW ); /* test the LEDs at the same time */ led_off( LED_ALL ); i = 0; while (i<3) { led_toggle( LED_RED ); led_toggle( LED_YELLOW ); led_toggle( LED_GREEN ); hal_delay(500); i++; } /* testing LED twinkle * there's a infinite loop in led_twinkle, so it will never come out*/ led_off( LED_ALL ); // led_twinkle( LED_RED, 300, 5 ); // led_off( LED_RED ); led_twinkle( LED_YELLOW, 300, 5 ); led_off( LED_YELLOW ); // led_twinkle( LED_GREEN, 300, 5 ); // led_off( LED_GREEN ); }
AbiBindMsgIMU_ACCEL_INT32(ABI_BROADCAST, &accel_ev, accel_cb); AbiBindMsgIMU_MAG_INT32(ABI_BROADCAST, &mag_ev, mag_cb); } static inline void led_toggle(void) { #ifdef BOARD_LISA_L LED_TOGGLE(7); #endif } static inline void main_periodic_task(void) { RunOnceEvery(100, { led_toggle(); DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); }); #if USE_I2C1 RunOnceEvery(111, { uint16_t i2c1_wd_reset_cnt = i2c1.errors->wd_reset_cnt; uint16_t i2c1_queue_full_cnt = i2c1.errors->queue_full_cnt; uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt; uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt; uint16_t i2c1_arb_lost_cnt = i2c1.errors->arb_lost_cnt; uint16_t i2c1_over_under_cnt = i2c1.errors->over_under_cnt; uint16_t i2c1_pec_recep_cnt = i2c1.errors->pec_recep_cnt; uint16_t i2c1_timeout_tlow_cnt = i2c1.errors->timeout_tlow_cnt; uint16_t i2c1_smbus_alert_cnt = i2c1.errors->smbus_alert_cnt; uint16_t i2c1_unexpected_event_cnt = i2c1.errors->unexpected_event_cnt;
int16_t main(void) { init_pin(); init_clock(); init_uart(); init_ui(); init_timer(); init_oc(); //setup the signal input pin pin_digitalIn(&D[4]); val1 = 0; val2 = 0; pos = 0; //16 bit int with binary point in front of the MSB led_on(&led2); timer_setPeriod(&timer2, PULSE_FREQUENCY); //how often we send a pulse timer_start(&timer2); timer_setPeriod(&timer3, 0.5); //heartbeat timer_start(&timer3); oc_servo(&oc1,&D[0],&timer4, INTERVAL,MIN_WIDTH, MAX_WIDTH, pos); oc_servo(&oc2,&D[2],&timer5, INTERVAL,MIN_WIDTH, MAX_WIDTH, pos); oc_pwm(&oc3,&D[3],NULL,FREQ,ZERO_DUTY); printf("Good morning\n"); InitUSB(); // initialize the USB registers and serial interface engine while (USB_USWSTAT!=CONFIG_STATE) { // while the peripheral is not configured... ServiceUSB(); // ...service USB requests } while (1) { ServiceUSB(); pin_write(&D[0],val1); pin_write(&D[2],val2); //adapted from Patrick and Charlie's approach if (!send_pulse && timer_read(&timer2) < PULSE_WIDTH){ send_pulse = 1; pin_write(&D[3],HALF_DUTY); get_distance = 1; } else if (send_pulse && timer_read(&timer2) >= PULSE_WIDTH) { send_pulse = 0; pin_write(&D[3],ZERO_DUTY); } if (timer_read(&timer2) >= ECHO_TIME) { if (pin_read(&D[4]) && get_distance) { printf("%d\n", timer_read(&timer2)); get_distance = 0; } } if (timer_flag(&timer3)) { //show a heartbeat and a status message timer_lower(&timer3); led_toggle(&led1); } } }
// Timer fired handler static void timer_handler (void* p_context) { //led_toggle(BLEES_LED_PIN); led_toggle(LED_0); }
void userbutton_callback(button_id_t button_id) { log_print_string("Button PB%u pressed.", button_id); console_print("Button Pressed\r\n"); led_toggle(0); }
int main(void) { uint16 value, count; uint8 len; char * request; char * response; char * payload; char * msg = "welcome to node..."; TiTimerAdapter * timeradapter; TiTimerManager * vtm; TiTimer * mac_timer; TiCc2420Adapter * cc; TiFrameRxTxInterface * rxtx; TiNioAcceptor * nac; TiAloha * mac; TiAdcAdapter * adc; TiLumSensor * lum; TiDataTreeNetwork * dtp; TiFrame * rxbuf; TiFrame * txbuf; target_init(); led_open(); led_on( LED_ALL ); hal_delay( 500 ); led_off( LED_ALL ); rtl_init( (void *)dbio_open(38400), (TiFunDebugIoPutChar)dbio_putchar, (TiFunDebugIoGetChar)dbio_getchar, hal_assert_report ); dbc_write( msg, strlen(msg) ); timeradapter = timer_construct( (void *)(&m_timeradapter), sizeof(m_timeradapter) ); vtm = vtm_construct( (void*)&m_vtm, sizeof(m_vtm) ); cc = cc2420_construct( (char *)(&m_cc), sizeof(TiCc2420Adapter) ); nac = nac_construct( &m_nacmem[0], NAC_SIZE ); mac = aloha_construct( (char *)(&m_aloha), sizeof(TiAloha) ); dtp = dtp_construct( (char *)(&m_dtp), sizeof(TiDataTreeNetwork) ); adc = adc_construct( (void *)&m_adc, sizeof(TiAdcAdapter) ); lum = lum_construct( (void *)&m_lum, sizeof(TiLumSensor) ); txbuf = frame_open( (char*)(&m_txbuf), FRAME_HOPESIZE(MAX_IEEE802FRAME154_SIZE), 3, 20, 0 ); rxbuf = frame_open( (char*)(&m_rxbuf), FRAME_HOPESIZE(MAX_IEEE802FRAME154_SIZE), 3, 20, 0 ); // timeradapter is used by the vtm(virtual timer manager). vtm require to enable the // period interrupt modal of vtm //timeradapter = timer_open( timeradapter, 0, NULL, NULL, 0x01 ); vtm = vtm_open( vtm, timeradapter, CONFIG_VTM_RESOLUTION ); cc = cc2420_open(cc, 0, NULL, NULL, 0x00 ); rxtx = cc2420_interface( cc, &m_rxtx ); mac_timer = vtm_apply( vtm ); mac_timer = vti_open( mac_timer, NULL, mac_timer); hal_assert( rxtx != NULL ); nac = nac_open( nac, rxtx, CONFIG_NIOACCEPTOR_RXQUE_CAPACITY, CONFIG_NIOACCEPTOR_TXQUE_CAPACITY); hal_assert( nac != NULL ); mac = aloha_open( mac, rxtx,nac, CONFIG_NODE_CHANNEL, CONFIG_NODE_PANID, CONFIG_NODE_ADDRESS,mac_timer, NULL, NULL,0x01); adc = adc_open( adc, 0, NULL, NULL, 0 ); lum = lum_open( lum, 0, adc ); dtp = dtp_open( dtp, mac, CONFIG_NODE_ADDRESS, NULL, NULL, 0x00 ); //todo cc2420_setchannel( cc, CONFIG_NODE_CHANNEL ); cc2420_setrxmode( cc ); // enable RX mode cc2420_setpanid( cc, CONFIG_NODE_PANID ); // network identifier, seems no use in sniffer mode cc2420_setshortaddress( cc, CONFIG_NODE_ADDRESS ); // in network address, seems no use in sniffer mode cc2420_enable_autoack( cc ); //todo cc2420_settxpower( cc, CC2420_POWER_1);//cc2420_settxpower( cc, CC2420_POWER_2); cc2420_enable_autoack( cc ); // ledtune = ledtune_construct( (void*)(&m_ledtune), sizeof(m_ledtune), vti ); // ledtune = ledtune_open( ledtune ); /* assert: all the above open() functions return non NULL values */ hal_assert((timeradapter != NULL) && (cc != NULL) && (mac != NULL) && (adc != NULL) && (lum != NULL) && (rxbuf != NULL) && (txbuf != NULL) && (dtp != NULL)); hal_enable_interrupts(); dtp->state = DTP_STATE_IDLE;//todo for testing 临时用这一句必须删掉 //todo for testing //dtp->root = 0x01;//todo for testing //dtp->parent = 0x03;//todo for testing /* while ( 1)//todo for testing { response = frame_startptr( txbuf ); value = lum_value( lum ); payload = DTP_PAYLOAD_PTR(response); payload[0] = 0x13; payload[1] = 0x14; if (dtp_send_response(dtp, txbuf, 0x01) > 0) { led_toggle( LED_RED);//todo for testing } dtp_evolve( dtp, NULL ); hal_delay( 2000);//todo for testing } */ while(1) { /* Only the following two kinds of frames will be put into "rxbuf" by dtp_recv() * - broadcast frames. the destination address of these frames are 0xFFFF. * - destination is the current node. */ //dbo_putchar(0x33); len = dtp_recv( dtp, rxbuf, 0x00 ); if (len > 0) { //ieee802frame154_dump( rxbuf); request = frame_startptr( rxbuf ); switch (DTP_CMDTYPE(request)) { /* if the frame is DTP_DATA_REQUEST, then the node will measure the data and * encapsulate the data into the txbuf, which is a TiOpenFrame and sent it back. */ case DTP_DATA_REQUEST: //payload = DTP_PAYLOAD_PTR( frame_startptr(txbuf) ); //ledtune_write( ledtune, MAKE_WORD(payload[1], payload[0]) ); // response frame = PHY Length 1B // + Frame Control 2B // + Sequence No 1B // + Destination Pan & Address 4B // + Source Pan & Address 4B // + DTP Section 15B //opf_cast( txbuf, 50, OPF_DEF_FRAMECONTROL_DATA_ACK ); response = frame_startptr( txbuf ); value = lum_value( lum ); DTP_SET_MAX_HOPCOUNT( response,0x03);//todo for testing payload = DTP_PAYLOAD_PTR(response); //payload[0] = 0x17;//todo 第三个节点数据 //payload[1] = 0x18;//todo 第三个节点数据 //payload[1] = 0x13; //payload[2] = 0x14; payload[1] = 0x15;//todo 另一个节点 payload[2] = 0x16;//todo 另一个节点 /* call dtp_send_response() to send the data in txbuf out. * * modified by zhangwei on 20091230 * - Bug fix. In the past, there's no delay between two adjacent * dtp_send_response() calls. This policy is too ambitious and this * node will occupy the whole time so that the other nodes will lost * chances to send, or encounter much higher frame collision probabilities. * so I add a little time delay here. * Attention the delay time here shouldn't be too large because * we don't want the hal_delay() to occupy all the CPU time. If this * occurs, it may lead to unnecessary frame lossing. */ // try some times until the RESPONSE is successfully sent for (count=0; count<10; count++) { //hal_delay( 500); if (dtp_send_response(dtp, txbuf, 0x03) > 0) { led_toggle( LED_RED);//todo for testing break; } //hal_delay( 50 ); } break; default: //hal_assert(false); break; } } nac_evolve( nac,NULL);//todo for tesitng aloha_evolve( mac,NULL);//todo for testing dtp_evolve( dtp, NULL ); hal_delay( 50 ); } }
mp_obj_t led_obj_toggle(mp_obj_t self_in) { pyb_led_obj_t *self = self_in; led_toggle(self->led_id); return mp_const_none; }
/// \method toggle() /// Toggle the LED between on and off. mp_obj_t led_obj_toggle(mp_obj_t self_in) { board_led_obj_t *self = self_in; led_toggle(self); return mp_const_none; }
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { led_toggle(); usb_tim_cb(htim); }
int main(void) { bool_t isUserRun = FALSE; bool_t displayCountdown = TRUE; uint16_t bootByte; _ee_getReserved(_AI_EE_RES_ADDR_BOOT, &bootByte); if (button_get(0) && button_get(1)) { // Both buttons are pressed so the user // wants to run the bootloader. isUserRun = TRUE; } else if (bootByte != _AI_EE_RES_VAL_BOOT_RUN) { startProgram(); } _ee_putReserved(_AI_EE_RES_ADDR_BOOT, _AI_EE_RES_VAL_DEFAULT); led_on(0); led_on(1); int i, j; for (i = 0; i < BOOT_TIMEOUT; i++) { if (isUserRun) { // flash LED1 if (i % 100 == 0) led_toggle(1); // update the countdown if (i % 1000 == 0) { lcd_clear(); lcd_printf("Aithon Board\n%d", (BOOT_TIMEOUT-i)/1000); displayCountdown = TRUE; } // show the bootloader build date if button 0 pressed if (button_get(0) && displayCountdown) { if (i > (.1 * BOOT_TIMEOUT)) { lcd_clear(); lcd_printf(DATE); displayCountdown = FALSE; } } } // check all the interfaces for a SYNC for (j = 0; j < NUM_INTERFACES; j++) { if (sdGetTimeout(_interfaces[j], TIME_IMMEDIATE) == SYNC) { _interface = _interfaces[j]; updateProgram(); // We should never get here... while(1); } } chThdSleepMilliseconds(1); } startProgram(); return 0; }
/******************************************************************* * MAIN() *******************************************************************/ int main(void) { long lEEPROMRetStatus; uint16_t i=0; uint8_t halted_latch = 0; // Set the clocking to run at 80 MHz from the PLL. // (Well we were at 80MHz with SYSCTL_SYSDIV_2_5 but according to the errata you can't // write to FLASH at frequencies greater than 50MHz so I slowed it down. I supposed we // could slow the clock down when writing to FLASH but then we need to find out how long // it takes for the clock to stabilize. This is on at the bottom of my list of things to do // for now) SysCtlClockSet(SYSCTL_SYSDIV_4_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN); // Initialize the device pinout. SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOJ); // Enable processor interrupts. IntMasterEnable(); // Setup the UART's my_uart_0_init(115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)); // command_handler_init overwrites the baud rate. We still need to configure the pins though my_uart_1_init(38400, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)); // Enable the command handler command_handler_init(); // We set the baud in here // Start the timers my_timer0_init(); my_timer1_init(); i2c_init(); motor_init(); qei_init(); gyro_init(); accel_init(); led_init(); //rc_radio_init(); //setupBluetooth(); // Initialize the EEPROM emulation region. lEEPROMRetStatus = SoftEEPROMInit(EEPROM_START_ADDR, EEPROM_END_ADDR, EEPROM_PAGE_SIZE); if(lEEPROMRetStatus != 0) UART0Send("EEprom ERROR!\n", 14); #if 0 // If ever we wanted to write some parameters to FLASH without the HMI // we could do it here. SoftEEPROMWriteDouble(kP_ID, 10.00); SoftEEPROMWriteDouble(kI_ID, 10.00); SoftEEPROMWriteDouble(kD_ID, 10.00); SoftEEPROMWriteDouble(ANG_ID, 0.0); SoftEEPROMWriteDouble(COMPC_ID, 0.99); #endif kP = SoftEEPROMReadDouble(kP_ID); kI = SoftEEPROMReadDouble(kI_ID); kD = SoftEEPROMReadDouble(kD_ID); commanded_ang = zero_ang = SoftEEPROMReadDouble(ANG_ID); COMP_C = SoftEEPROMReadDouble(COMPC_ID); pid_init(kP, kI, kD, &pid_ang); motor_controller_init(20, 100, 10, &mot_left); motor_controller_init(20, 100, 10, &mot_right); //pid_init(0.0, 0.0, 0.0, &pid_pos_left); //pid_init(0.0, 0.0, 0.0, &pid_pos_right); //UART0Send("Hello World!\n", 13); // Tell the HMI what the initial parameters are. print_params(1); while(1) { delta_t = myTimerValueGet(); myTimerZero(); sum_delta_t += delta_t; // Read our sensors accel_get_xyz_cal(&accel_x, &accel_y, &accel_z, true); gyro_get_y_cal(&gyro_y, false); // Calculate the pitch angle with the accelerometer only R = sqrt(pow(accel_x, 2) + pow(accel_z, 2)); accel_pitch_ang = (acos(accel_z / R)*(RAD_TO_DEG)) - 90.0 - zero_ang; //accel_pitch_ang = (double)((atan2(accel_x, -accel_z))*RAD_TO_DEG - 90.0); gyro_pitch_ang += (double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t); // Kalman filter //filtered_ang = kalman((double)accel_pitch_ang, ((double)gyro_y)*g_gyroScale, CONV_TO_SEC(delta_t)); filtered_ang = (COMP_C*(filtered_ang+((double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t)))) + ((1.0-COMP_C)*(double)accel_pitch_ang); // Skip the rest of the process until the angle stabilizes if(i < 250) { i++; continue; } // Tell the HMI what's going on every 100ms if(sum_delta_t >= 1000) { print_update(1); print_debug(0); //print_control_surfaces(0); led_toggle(); //print_angle(); sum_delta_t = 0; } // See if the HMI has anything to say command_handler(); //continue; // If we are leaning more than +/- FALL_ANG deg off center it's hopeless. // Turn off the motors in hopes of some damage control if( abs(filtered_ang) > FALL_ANG ) { if(halted_latch) continue; stop_motors(); halted_latch = 1; continue; } halted_latch = 0; motor_val = pid_controller(calc_commanded_angle(0), filtered_ang, delta_t, &pid_ang); motor_left = motor_right = motor_val; drive_motors(motor_left*left_mot_gain, motor_right*right_mot_gain); } }
void timer1_callback() { led_toggle(1); timer_post_task_delay(&timer1_callback, 0x0000FFFF + (uint32_t)100); log_print_string("Toggled led %d", 1); }