void main(void) { //configuring P1OUT |= 0x04; // set P1.2 for debug P4DIR |= 0x20; // P4.5 as output (for debug) gina_init(); scheduler_init(); leds_init(); if (*(&eui64+3)==0x09) { // this is a GINA board (not a basestation) gyro_init(); large_range_accel_init(); magnetometer_init(); sensitive_accel_temperature_init(); } radio_init(); timer_init(); P1OUT &= ~0x04; // clear P1.2 for debug //check sensor configuration is right gyro_get_config(); large_range_accel_get_config(); magnetometer_get_config(); sensitive_accel_temperature_get_config(); //scheduler_push_task(ID_TASK_APPLICATION); scheduler_register_application_task(&task_application_imu_radio, 0, FALSE); scheduler_start(); }
int main(int argc, char **argv) { /* 模块初始化 */ exc_init(); /* 中断初始化 */ sys_timer_init(); /* 系统时钟初始化 */ light_init(); /* LED灯初始化 */ switch_init(); /* 开关初始化 */ speaker_init(); /* 蜂鸣器初始化 */ motor_init(); /* 电机初始化 */ decoder_init(); /* 编码器初始化 */ gyro_init(); /* 陀螺仪初始化 */ acc_init(); /* 加速度传感器初始化 */ serial_initialize((intptr_t)(NULL)); /* 初始化串口 */ //sd_init(&Fatfs); /* 初始化SD卡,并创建文件 */ //sd_create_file(&test_data, test_data_name); /* 命令注册 */ help_cmd_initialize((intptr_t)(NULL)); light_cmd_initialize((intptr_t)(NULL)); switch_cmd_initialize((intptr_t)(NULL)); speaker_cmd_initialize((intptr_t)(NULL)); motor_cmd_initialize((intptr_t)(NULL)); decoder_cmd_initialize((intptr_t)(NULL)); //sd_cmd_initialize((intptr_t)(NULL)); printf("\n Welcome to k60 software platform!"); printf("\n Press 'help' to get the help! \n"); light_open(LIGHT4); /* ntshell测试 */ task_ntshell((intptr_t)(NULL)); }
static int gyroscope_l3g4200d_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { struct gyroscope_l3g4200d_data *mdata = data; const struct sol_flow_node_type_gyroscope_l3g4200d_options *opts = (const struct sol_flow_node_type_gyroscope_l3g4200d_options *)options; SOL_NULL_CHECK(options, -EINVAL); mdata->i2c = sol_i2c_open(opts->i2c_bus.val, I2C_SPEED); if (!mdata->i2c) { SOL_WRN("Failed to open i2c bus"); return -EBADR; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return -EIO; } mdata->use_rad = opts->output_radians; mdata->init_sampling_cnt = 3; mdata->ready = false; mdata->node = node; return gyro_init(mdata); }
void init() { bSmartDiagnostics = true; //true to enable smart diagnostic screen bCompetitionMode = true; //true to enable competition mode displaySplash("Mecanum Bot", "", true); bool ok = false; while(!ok) { const int testcount = 2; bool test[testcount] = { errorcheck(1,0,1,MOTORCON), errorcheck(1,0,2,MOTORCON)}; string desc[testcount] = {"MC1-1","MC1-2"}; ok = error_display(test,desc,testcount); if (!ok) { PlayTone(440, 50); if (test[0] == false && test[1] == false){ nxtDisplayCenteredTextLine(7, "Reboot MC!"); } } else { ClearSounds(); } } eraseDisplay(); gyro_init(HTGYRO); wait1Msec(50); nxtbarOn(); return; }
int usetup (void) { extern volatile uint8_t robot_id; robot_id = 8; territory_init(); gyro_init(PIN_GYRO, LSB_US_PER_DEG, 500); return 0; }
// usetup is called during the calibration period. It must return before the // period ends. int usetup (void) { printf("\nPlace robot, press go."); go_click (); printf ("\nStabilizing..."); pause (500); printf ("\nCalibrating offset...\n"); gyro_init (GYRO_PORT, LSB_US_PER_DEG, 500L); return 0; }
int usetup (void) { extern volatile uint8_t robot_id; robot_id = 8; gyro_init(PIN_GYRO, LSB_US_PER_DEG, 500); printf("starting daemons..."); // vps_data_daemon_init(); hybrid_position_daemon_init(); printf(" done\n"); return 0; }
void init() { bSmartDiagnostics = true; //true to enable smart diagnostic screen bCompetitionMode = false; //true to enable competition mode displaySplash("Mecanum Bot", "FOD Test", true); eraseDisplay(); gyro_init(HTGYRO); wait1Msec(50); nxtbarOn(); return; }
void i2cdriver_init(void) { i2cStart(&I2CD1, &i2c_conf); // Start the I2C1 driver debug_println("Init: I2C Driver Active"); accel_init(); // Prepare the accelerometer debug_println("Init: Accelerometer Active"); chThdSleepMilliseconds(100); // May not be needed. Using as in example code. gyro_init(); // Prepare the gyroscope debug_println("Init: Gyroscope Active"); chThdSleepMilliseconds(100); // Also may not be needed. }
imu::imu(void) { twi_init(); // Initialize all peripherals. accl_init(); mag_init(); gyro_init(); gyro_roll_angle = 0; // Initialize internal variables to zero. gyro_pitch_angle = 0; roll_angle = 0; pitch_angle = 0; yaw_angle = 0; //gyro_x_position(void) }
int usetup (void) { printf("\nPlace robot, press go."); go_click (); printf ("\nStabilizing..."); pause (500); printf ("\nCalibrating offset...\n"); gyro_init (GYRO_PORT, LSB_US_PER_DEG, 500L); extern volatile uint8_t robot_id; robot_id = 128; extern volatile uint8_t light_port; light_port = 4; return 0; }
void init (void) { //1 = output, 0 = input DDRB = 0b01100000; //PORTB4, B5 output for stat LED DDRC = 0b00010000; //PORTC4 (SDA), PORTC5 (SCL), PORTC all others are inputs DDRD = 0b00000010; //PORTD (TX output on PD1) PORTC = 0b00110000; //pullups on the I2C bus cbi(PORTB, 5); i2cInit(); accelerometer_init(); magnetometer_init(); gyro_init(); check_baud(); }
void main(void) { WDTCTL = WDTPW + WDTHOLD; // disable watchdog timer BCSCTL1 = CALBC1_16MHZ; // MCLK at 16MHz DCOCTL = CALDCO_16MHZ; P1DIR |= 0x1E; // P1.1-4 as outputs (for debug) __enable_interrupt(); // global enable interrupts leds_init(); //configuring the gyro if (*(&eui64+3)==0x09) { // this is a GINA board (not a basestation) i2c_init(): gyro_init(); }
void setup_state() { while (state == SETUP) { create_thread(sing, 64, 127, "sing_thread"); printf("\nPress go"); go_click(); printf("\nStabilizing"); pause(1000); printf("\nInitializing"); gyro_init(8,1357.348162*3838.0/3600.0*1028.0/1080.0,5000); target_angle = 0; reset_pid_controller(target_angle); setup_filter(); } }
void init() { servo[TubeWinch] = 127; servo[GoalRetainer] = 5; servo[Kickstand] = 155; servo[BallStorage] = 80; servo[TouchSensor] = 65; bSmartDiagnostics = true; //true to enable smart diagnostic screen bCompetitionMode = true; //true to enable competition mode log_enabled = true; //Enable logging in Teleop displaySplash("High PHidelity", "Teleop", true); eraseDisplay(); gyro_init(HTGYRO); wait1Msec(50); nxtbarOn(); return; }
int usetup (void) { extern volatile uint8_t robot_id; robot_id = 8; printf("stabilizing... "); pause(700); printf("done\n"); territory_init(); printf("calibrating gyro... "); gyro_init(PIN_GYRO, LSB_US_PER_DEG, 1000); printf("done\n"); vps_data_daemon_init(); pause(300); printf("binding gyro to vps coordinates... "); gyro_set_degrees(get_vps_theta()); printf("done\n"); determine_team_color(); return 0; }
void board_init_devices(void) { #if defined(ENVIRONMENT_SENSOR) kputs("Initializing i2c\n"); i2c_init(); kputs("Initializing Devices... Light,"); light_init(); #ifdef REPORT_TYPE_GYRO kputs("Gyro, "); gyro_init(); #endif #ifdef REPORT_TYPE_ACCEL kputs("Accell, "); accel_init(); #endif #ifdef REPORT_TYPE_HUMID kputs("Humid, "); humid_init(); #endif kputs("PIR\n"); pir_init(); #elif defined POWER_STRIP_MONITOR kputs("Initializing Devices... Power\n"); powermon_init(); #endif #ifdef USE_PN532 kputs("Initializing RFID reader... \n"); rfid_init(); #endif #ifdef USE_MACHXO2_PMCO2 kputs("Initializing CO2 and PM extension...\n"); machxo2_init(); #endif #ifdef USE_DOOR_SENSORS kputs("Initializing Door Sensors...\n"); door_sensors_init(); #endif }
int sensors_init(void) { int ret; int ret_combined = 0; ret = accel_init(); if (ret) { ret_combined = ret; } ret = gyro_init(); if (ret) { ret_combined = ret; } ret = mag_init(); if (ret) { ret_combined = ret; } ret = baro_init(); if (ret) { ret_combined = ret; } return ret_combined; }
static int gyroscope_l3g4200d_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { struct gyroscope_l3g4200d_data *mdata = data; const struct sol_flow_node_type_gyroscope_l3g4200d_options *opts = (const struct sol_flow_node_type_gyroscope_l3g4200d_options *)options; SOL_FLOW_NODE_OPTIONS_SUB_API_CHECK(options, SOL_FLOW_NODE_TYPE_GYROSCOPE_L3G4200D_OPTIONS_API_VERSION, -EINVAL); mdata->i2c = sol_i2c_open(opts->i2c_bus, I2C_SPEED); SOL_NULL_CHECK_MSG(mdata->i2c, -EIO, "Failed to open i2c bus"); mdata->use_rad = opts->output_radians; mdata->init_sampling_cnt = 3; mdata->node = node; gyro_init(mdata); return 0; }
static void i2c_read_data_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; double scale = mdata->use_rad ? GYRO_SCALE_R_S * DEG_TO_RAD : GYRO_SCALE_R_S; uint8_t num_samples_available; struct sol_direction_vector val = { .min = -GYRO_RANGE, .max = GYRO_RANGE, .x = mdata->reading[0], .y = mdata->reading[1], .z = mdata->reading[2] }; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Failed to read L3G4200D gyro fifo status"); return; } num_samples_available = status / (sizeof(int16_t) * 3); /* raw readings, with only the sensor-provided filtering */ for (uint8_t i = 0; i < num_samples_available; i++) { mdata->reading[0] = mdata->gyro_data.buffer[i][0] * scale; mdata->reading[1] = -mdata->gyro_data.buffer[i][1] * scale; mdata->reading[2] = -mdata->gyro_data.buffer[i][2] * scale; } sol_flow_send_direction_vector_packet(mdata->node, SOL_FLOW_NODE_TYPE_GYROSCOPE_L3G4200D__OUT__OUT, &val); mdata->pending_ticks--; if (mdata->pending_ticks) gyro_tick_do(mdata); } static void i2c_read_fifo_status_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; uint8_t num_samples_available; uint8_t fifo_status = mdata->common.buffer[0]; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Failed to read L3G4200D gyro fifo status"); return; } if (fifo_status & GYRO_REG_FIFO_SRC_OVERRUN) { num_samples_available = 32; } else if (fifo_status & GYRO_REG_FIFO_SRC_EMPTY) { num_samples_available = 0; } else { num_samples_available = fifo_status & GYRO_REG_FIFO_SRC_ENTRIES_MASK; } if (!num_samples_available) { SOL_INF("No samples available"); return; } SOL_DBG("%d samples available", num_samples_available); /* Read *all* the entries in one go, using AUTO_INCREMENT */ mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_XL | GYRO_REG_AUTO_INCREMENT, (uint8_t *)&mdata->gyro_data.buffer[0][0], sizeof(mdata->gyro_data.buffer), i2c_read_data_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Failed to read L3G4200D gyro samples"); } static bool gyro_tick_do(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_tick_do); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } mdata->common.buffer[0] = 0; mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_FIFO_SRC, mdata->common.buffer, 1, i2c_read_fifo_status_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Failed to read L3G4200D gyro fifo status"); return false; } static bool gyro_ready(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->ready = true; SOL_DBG("gyro is ready for reading"); if (mdata->pending_ticks) gyro_tick_do(mdata); return false; } static void i2c_write_fifo_ctl_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's stream mode"); return; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_ready) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } static bool gyro_init_stream(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_stream); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* enable FIFO in stream mode */ mdata->common.buffer[0] = GYRO_REG_FIFO_CTL_STREAM; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_FIFO_CTL, mdata->common.buffer, 1, i2c_write_fifo_ctl_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's stream mode"); return false; } static void i2c_write_ctrl_reg5_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's fifo mode"); return; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_stream) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } static bool gyro_init_fifo(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_fifo); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } mdata->common.buffer[0] = GYRO_REG_CTRL_REG5_FIFO_EN; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG5, mdata->common.buffer, 1, i2c_write_ctrl_reg5_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's fifo mode"); return false; } static void i2c_write_ctrl_reg4_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's resolution"); return; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_fifo) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } static bool gyro_init_range(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_range); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* setup for 2000 degrees/sec */ mdata->common.buffer[0] = GYRO_REG_CTRL_REG4_FS_2000; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG4, mdata->common.buffer, 1, i2c_write_ctrl_reg4_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's resolution"); return false; } static bool gyro_init_sampling(void *data); static void i2c_write_ctrl_reg1_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's sampling rate"); return; } mdata->init_sampling_cnt--; if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, mdata->init_sampling_cnt ? gyro_init_sampling : gyro_init_range) < 0) { SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } } /* meant to run 3 times */ static bool gyro_init_sampling(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_sampling); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* setup for 800Hz sampling with 110Hz filter */ mdata->common.buffer[0] = GYRO_REG_CTRL_REG1_DRBW_800_110 | GYRO_REG_CTRL_REG1_PD | GYRO_REG_CTRL_REG1_XYZ_ENABLE; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG1, mdata->common.buffer, 1, i2c_write_ctrl_reg1_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's sampling rate"); return false; } static void i2c_read_who_am_i_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Failed to read i2c register"); return; } if (mdata->common.buffer[0] != GYRO_REG_WHO_AM_I_VALUE) { SOL_WRN("could not find L3G4200D gyro sensor"); return; } gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_sampling); } static bool gyro_init(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_WHO_AM_I, mdata->common.buffer, 1, i2c_read_who_am_i_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Failed to read i2c register"); return false; } static int gyroscope_l3g4200d_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { struct gyroscope_l3g4200d_data *mdata = data; const struct sol_flow_node_type_gyroscope_l3g4200d_options *opts = (const struct sol_flow_node_type_gyroscope_l3g4200d_options *)options; SOL_NULL_CHECK(options, -EINVAL); mdata->i2c = sol_i2c_open(opts->i2c_bus, I2C_SPEED); SOL_NULL_CHECK_MSG(mdata->i2c, -EIO, "Failed to open i2c bus"); mdata->use_rad = opts->output_radians; mdata->init_sampling_cnt = 3; mdata->node = node; gyro_init(mdata); return 0; } static void gyroscope_l3g4200d_close(struct sol_flow_node *node, void *data) { struct gyroscope_l3g4200d_data *mdata = data; if (mdata->i2c_pending) sol_i2c_pending_cancel(mdata->i2c, mdata->i2c_pending); if (mdata->i2c) sol_i2c_close(mdata->i2c); if (mdata->timer) sol_timeout_del(mdata->timer); } static int gyroscope_l3g4200d_tick(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet) { struct gyroscope_l3g4200d_data *mdata = data; if (!mdata->ready || mdata->pending_ticks) { mdata->pending_ticks++; return 0; } gyro_tick_do(mdata); return 0; }
int usetup(void){ robot_id = 1; //team 1 gyro_init(GYRO_PORT, GYRO_DEGREE_CONVERSION, GYRO_CALIB_WAIT_TIME); return 0; }
void init_ap( void ) { #ifndef SINGLE_MCU /** init done in main_fbw in single MCU */ mcu_init(); sys_time_init(); #endif /* SINGLE_MCU */ /************* Sensors initialization ***************/ #ifdef USE_INFRARED infrared_init(); #endif #ifdef USE_GYRO gyro_init(); #endif #ifdef USE_GPS gps_init(); #endif #ifdef USE_GPIO GpioInit(); #endif #ifdef USE_IMU imu_init(); #endif #ifdef USE_AHRS ahrs_aligner_init(); ahrs_init(); #endif /************* Links initialization ***************/ #if defined MCU_SPI_LINK link_mcu_init(); #endif #ifdef MODEM modem_init(); #endif /************ Internal status ***************/ h_ctl_init(); v_ctl_init(); estimator_init(); #ifdef ALT_KALMAN alt_kalman_init(); #endif nav_init(); modules_init(); settings_init(); /** - start interrupt task */ mcu_int_enable(); /** wait 0.5s (historical :-) */ sys_time_usleep(500000); #if defined GPS_CONFIGURE gps_configure_uart(); #endif #if defined DATALINK #if DATALINK == XBEE xbee_init(); #endif #endif /* DATALINK */ #if defined AEROCOMM_DATA_PIN IO0DIR |= _BV(AEROCOMM_DATA_PIN); IO0SET = _BV(AEROCOMM_DATA_PIN); #endif power_switch = FALSE; /************ Multi-uavs status ***************/ #ifdef TRAFFIC_INFO traffic_info_init(); #endif }
void Sensors::task_main() { /* start individual sensors */ accel_init(); gyro_init(); mag_init(); baro_init(); adc_init(); /* * do subscriptions */ _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ orb_set_interval(_gyro_sub, 4); /* * do advertisements */ struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); raw.timestamp = hrt_absolute_time(); raw.adc_voltage_v[0] = 0.0f; raw.adc_voltage_v[1] = 0.0f; raw.adc_voltage_v[2] = 0.0f; raw.adc_voltage_v[3] = 0.0f; memset(&_battery_status, 0, sizeof(_battery_status)); _battery_status.voltage_v = -1.0f; _battery_status.voltage_filtered_v = -1.0f; _battery_status.current_a = -1.0f; _battery_status.discharged_mah = -1.0f; /* get a set of initial values */ accel_poll(raw); gyro_poll(raw); mag_poll(raw); baro_poll(raw); diff_pres_poll(raw); parameter_update_poll(true /* forced */); /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); /* wakeup source(s) */ struct pollfd fds[1]; /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ fds[0].fd = _gyro_sub; fds[0].events = POLLIN; while (!_task_should_exit) { /* wait for up to 50ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); /* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */ /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ vehicle_control_mode_poll(); /* check parameters for updates */ parameter_update_poll(); /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ gyro_poll(raw); accel_poll(raw); mag_poll(raw); baro_poll(raw); /* check battery voltage */ adc_poll(raw); diff_pres_poll(raw); /* Inform other processes that new data is available to copy */ if (_publishing) { orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); } /* Look for new r/c input data */ rc_poll(); perf_end(_loop_perf); } warnx("[sensors] exiting."); _sensors_task = -1; _exit(0); }
int main(int argc, char *argv[]) { data_t accel_data, gyro_data; data_t gyro_offset; float a_res, g_res; mraa_i2c_context accel, gyro; accel_scale_t a_scale = A_SCALE_8G; gyro_scale_t g_scale = G_SCALE_2000DPS; int client_socket_fd, portno, n; struct sockaddr_in serv_addr; struct hostent *server; int flags; float buffer[BUFF_SIZE]; int send_idx = 0, curr_idx = 0; int next_it, send_it, curr_it; // helper variables for converting idx (byte) to it (float) int num_pts = 0; int i; int init_sam = (SAMP_RATE*INIT_SEC)+1; // number of samples during init period int quit = 0; // quit flag char stop; // dummy buffer for stop ping // Read command line arguments, need to get the host IP address and port if (argc < 3) { fprintf(stderr, "USAGE: %s <hostname> <port>\n", argv[0]); exit(1); } // Convert the arguments to the appropriate data types portno = atoi(argv[2]); // setup the socket // technically PF_INET. Refer to: // http://stackoverflow.com/questions/6729366/what-is-the-difference-between-af-inet-and-pf-inet-in-socket-programming //client_socket_fd = socket(AF_INET, SOCK_STREAM, 0); client_socket_fd = socket(PF_INET, SOCK_STREAM, 0); // check if the socket was created successfully. If it wasnt, display an error and exit if(client_socket_fd < 0) { error("ERROR opening socket"); } // check if the IP entered by the user is valid server = gethostbyname(argv[1]); if (server == NULL) { fprintf(stderr,"ERROR, no such host\n"); exit(1); } // clear our the serv_addr buffer memset((char *) &serv_addr, 0, sizeof(serv_addr)); // set up the socket serv_addr.sin_family = AF_INET; memcpy((char *)&serv_addr.sin_addr.s_addr, (char *)server->h_addr, server->h_length); serv_addr.sin_port = htons(portno); // try to connect to the server if (connect(client_socket_fd,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0){ error("ERROR connecting"); } // make non-blocking. Refer to: // http://developerweb.net/viewtopic.php?id=3000 /* Set socket to non-blocking */ if ((flags = fcntl(client_socket_fd, F_GETFL, 0)) < 0) { error("ERROR getting socket flags"); } if (fcntl(client_socket_fd, F_SETFL, flags | O_NONBLOCK) < 0) { error("ERROR setting socket to non-blocking"); } printf("Socket connected to server!\n"); printf("Now beginning sensor initialization\n"); // initialize sensors, set scale, and calculate resolution. accel = accel_init(); set_accel_scale(accel, a_scale); a_res = calc_accel_res(a_scale); gyro = gyro_init(); set_gyro_scale(gyro, g_scale); g_res = calc_gyro_res(g_scale); gyro_offset = calc_gyro_offset(gyro, g_res); printf("GYRO OFFSETS x: %f y: %f z: %f\n", gyro_offset.x, gyro_offset.y, gyro_offset.z); //Read the sensor data and print them. printf("STARTING TO COLLECT DATA\n"); printf("\n \t\tAccelerometer\t\t\t||"); printf("\t\t\tGyroscope\t\t\t\n"); while (1) { for (i = 0; i < PACK_PTS; i++) { n = read(client_socket_fd, &stop, 1); if (n < 0) { if (!(errno == EWOULDBLOCK || errno == EAGAIN)) error("ERROR reading from client socket"); // else no stop ping yet } else { #ifdef DEBUG printf("RECEIVED STOP PING\n"); #endif quit = 1; break; } if (num_pts == init_sam) printf("INITIALIZATION PERIOD DONE\n"); accel_data = read_accel(accel, a_res); gyro_data = read_gyro(gyro, g_res); if (num_pts%PRINT_PERIOD == 0) { printf("%i points\n", num_pts); printf(" send_idx: %d curr_idx: %d\n", send_idx, curr_idx); printf(" X: %f\t Y: %f\t Z: %f\t||", accel_data.x, accel_data.y, accel_data.z); printf("\tX: %f\t Y: %f\t Z: %f p: %d\n", gyro_data.x - gyro_offset.x, gyro_data.y - gyro_offset.y, gyro_data.z - gyro_offset.z, num_pts); } #ifdef DEBUG printf("X: %f\t Y: %f\t Z: %f\t||", accel_data.x, accel_data.y, accel_data.z); printf("\tX: %f\t Y: %f\t Z: %f p: %d\t\n", gyro_data.x - gyro_offset.x, gyro_data.y - gyro_offset.y, gyro_data.z - gyro_offset.z, num_pts); #endif curr_it = curr_idx/sizeof(float); buffer[curr_it] = gyro_data.x - gyro_offset.x; buffer[curr_it+1] = gyro_data.y - gyro_offset.y; buffer[curr_it+2] = gyro_data.z - gyro_offset.z; buffer[curr_it+3] = accel_data.x; buffer[curr_it+4] = accel_data.y; buffer[curr_it+5] = accel_data.z; num_pts++; next_it = (curr_it + SAMP_SIZE)%BUFF_SIZE; send_it = send_idx/sizeof(float); // truncate integer if ((next_it >= send_it) && (next_it <= (send_it+SAMP_SIZE-1))) { // note that curr_idx always advances SAMP_SIZE at a time // went around ring buffer. panic! fprintf(stderr, "Ring buffer is full. Abort!\n"); exit(1); } curr_idx = next_it*sizeof(float); } // batch done // stop writing packets if received stop ping if (quit) break; // write batch to server n = write(client_socket_fd, ((char*)buffer)+send_idx, ((curr_idx>send_idx)? (curr_idx-send_idx): (BUFF_BYTES-send_idx))); // n contains how many bytes were written to the socket // if n is less than 0, then there was an error // Refer to: // http://stackoverflow.com/questions/3153939/properly-writing-to-a-nonblocking-socket-in-c // http://beej.us/guide/bgnet/output/html/singlepage/bgnet.html if (n < 0) { if (errno == EWOULDBLOCK || errno == EAGAIN) { fprintf(stderr, "FAILED WRITE for byte %d. Currently at byte %d\n", send_idx, curr_idx); } else { error("ERROR writing to socket"); } } else { // successful write send_idx = (send_idx+n)%BUFF_BYTES; } #ifdef DEBUG printf("send_idx: %d curr_idx: %d\n", send_idx, curr_idx); #endif usleep(1000000/SAMP_RATE); } close(client_socket_fd); return 0; }
int main(int argc, char *argv[]) { /////VARIABLE DECLARATIONS///// //SENSORS mraa_i2c_context accel, gyro, mag; float a_res, g_res, m_res; data_t accel_data, gyro_data, mag_data; int16_t temperature; float pitch_angle, yaw_angle; char *x_accel_message; char *y_accel_message; char *z_accel_message; char *posture_message; int curr_posture; int prev_posture = 0; //SOCKETS AND MESSAGES int sockfd; //Socket descriptor int portno, n; char component[256]; char endpoint[] = "127.0.0.1"; struct sockaddr_in serv_addr; struct hostent *server; //struct containing a bunch of info char message[256]; int count; /////SENSOR INITIALIZATION AND SETUP accel = accel_init(); set_accel_scale(accel, A_SCALE_2G); set_accel_ODR(accel, A_ODR_100); a_res = calc_accel_res(A_SCALE_2G); gyro = gyro_init(); set_gyro_scale(gyro, G_SCALE_245DPS); set_gyro_ODR(accel, G_ODR_190_BW_70); g_res = calc_gyro_res(G_SCALE_245DPS); mag = mag_init(); set_mag_scale(mag, M_SCALE_2GS); set_mag_ODR(mag, M_ODR_125); m_res = calc_mag_res(M_SCALE_2GS); portno = 41234; //create socket sockfd = socket(AF_INET, SOCK_DGRAM, 0); //create a new socket if (sockfd < 0) error("ERROR opening socket"); server = gethostbyname("127.0.0.1"); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses... if (server == NULL) { fprintf(stderr,"ERROR, no such host\n"); exit(0); } //setup the server struct memset((char *) &serv_addr, 0, sizeof(serv_addr)); serv_addr.sin_family = AF_INET; //initialize server's address bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); serv_addr.sin_port = htons(portno); //connect to server if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server error("ERROR connecting"); //read accel and gyro data count = 0; while(1) { accel_data = read_accel(accel, a_res); gyro_data = read_gyro(gyro, g_res); //mag_data = read_mag(mag, m_res); //temperature = read_temp(accel); getAngles(accel_data, &pitch_angle, &yaw_angle); printf("is moving: %d\n", isMoving(gyro_data) ); //printf("yaw angle: %f ", yaw_angle); //printf("pitch angle: %f ", pitch_angle); //printf("z vector: %f\n", accel_data.z); if (count == 3) { //send posture to cloud if (isMoving(gyro_data)==0) //if patient is stationary, calculate new posture curr_posture = getPosture(accel_data, pitch_angle, yaw_angle); else curr_posture = prev_posture; //else just use the old posture if (curr_posture==UNDEFINED) curr_posture = prev_posture; prev_posture = curr_posture; //set new value for prev posture posture_message = construct_message(POS, accel_data, curr_posture); n = write(sockfd, posture_message, strlen(posture_message)); //write to the socket if (n < 0) error("ERROR writing to socket"); //store accel data in string /* x_accel_message = construct_message(X_DIR, accel_data); //printf("%s\n", full_message_x); //send UDP message n = write(sockfd,x_accel_message,strlen(x_accel_message)); //write to the socket if (n < 0) error("ERROR writing to socket"); y_accel_message = construct_message(Y_DIR, accel_data); //printf("%s\n", full_message_y); n = write(sockfd,y_accel_message,strlen(y_accel_message)); //write to the socket if (n < 0) error("ERROR writing to socket"); z_accel_message = construct_message(Z_DIR, accel_data); //printf("%s\n", full_message_z); n = write(sockfd,z_accel_message,strlen(z_accel_message)); //write to the socket if (n < 0) error("ERROR writing to socket"); */ count = 0; } char *posture_string; switch(curr_posture) { case 0: posture_string = "upright"; break; case 1: posture_string = "face up"; break; case 2: posture_string = "face down"; break; case 3: posture_string = "left"; break; case 4: posture_string = "right"; break; default: posture_string = "undefined"; } printf("current orientation: %s \n", posture_string); //printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z); //printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z); //printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z); //printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z); //printf("\t%ld\n", temperature); count++; usleep(100000); } 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); } }
int usetup (void) { robot_id = 10; gyro_init(GYRO_PORT, LSB_US_PER_DEG, 500L); return 0; }
int usetup (void) { gyro_init(PIN_GYRO, LSB_US_PER_DEG, 500); return 0; }
int main(int argc, char *argv[]) { /////VARIABLE DECLARATIONS///// //SENSORS mraa_i2c_context accel, gyro, mag; float a_res, g_res, m_res; data_t accel_data, gyro_data, mag_data, zero_rate; int16_t temperature; float pitch_angle, roll_angle, yaw_angle; char *x_accel_message; char *y_accel_message; char *z_accel_message; char posture_message[20]; int curr_posture; int prev_posture = 0; //SOCKETS AND MESSAGES int sockfd; //Socket descriptor int portno; char component[256]; char endpoint[] = "127.0.0.1"; struct sockaddr_in serv_addr; struct hostent *server; //struct containing a bunch of info char message[256]; char serv_message[256]; int count; int n, n1; /////SENSOR INITIALIZATION AND SETUP accel = accel_init(); set_accel_scale(accel, A_SCALE_2G); set_accel_ODR(accel, A_ODR_100); a_res = calc_accel_res(A_SCALE_2G); gyro = gyro_init(); set_gyro_scale(gyro, G_SCALE_245DPS); set_gyro_ODR(accel, G_ODR_190_BW_70); g_res = calc_gyro_res(G_SCALE_245DPS); mag = mag_init(); set_mag_scale(mag, M_SCALE_2GS); set_mag_ODR(mag, M_ODR_125); m_res = calc_mag_res(M_SCALE_2GS); zero_rate = calc_gyro_offset(gyro, g_res); portno = 2015; //create socket if (argc > 1) //if user supplies IP address as argument upperbodyIP = argv[1]; //take user input as upper body IP else upperbodyIP = "192.168.0.30"; sockfd = socket(AF_INET, SOCK_STREAM, 0); //create a new socket if (sockfd < 0) error("ERROR opening socket"); server = gethostbyname(upperbodyIP); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses... if (server == NULL) { fprintf(stderr,"ERROR, no such host\n"); exit(0); } //setup the server struct memset((char *) &serv_addr, 0, sizeof(serv_addr)); serv_addr.sin_family = AF_INET; //initialize server's address bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); serv_addr.sin_port = htons(portno); //connect to server if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server error("ERROR connecting"); //WAIT FOR THE OKAY BY THE SERVER// memset(serv_message, 0, 256); n = read(sockfd, serv_message, 256); if (strcmp(serv_message, "START") != 0) error("strange response from server"); //read accel and gyro data count = 0; while(1) { accel_data = read_accel(accel, a_res); gyro_data = read_gyro(gyro, g_res); //mag_data = read_mag(mag, m_res); //temperature = read_temp(accel); getAngles(accel_data, gyro_data, zero_rate, &pitch_angle, &roll_angle, &yaw_angle); printf("is moving: %d ", isMoving(gyro_data)); printf("yaw angle: %f ", yaw_angle); if (count == 3) { //send posture to main edison if (isMoving(gyro_data)==0) //if patient is stationary, calculate new posture curr_posture = getPosture(accel_data, pitch_angle, roll_angle); else curr_posture = UNDEFINED; //else just use the undefined/transition case prev_posture = curr_posture; //set new value for prev posture memset(posture_message, 0, sizeof(char)*20); snprintf(posture_message, 10, "%d,%f", curr_posture, yaw_angle); //posture_message = construct_message(POS, accel_data, curr_posture); printf("posture message: %s ", posture_message); n = write(sockfd, posture_message, strlen(posture_message)); //write to the socket if (n < 0) error("ERROR writing to socket"); /* bzero(message, 256); printf("waiting for response "); n1 = read(sockfd, message, 10); if (n1 < 0) error("ERROR reading from upper body"); printf("got response\n"); */ count = 0; } printPostureString(curr_posture); //printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z); //printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z); //printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z); //printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z); //printf("\t%ld\n", temperature); count++; usleep(100000); } return 0; }
void init_ap( void ) { #ifndef SINGLE_MCU /** init done in main_fbw in single MCU */ hw_init(); sys_time_init(); #ifdef LED led_init(); #endif #ifdef ADC adc_init(); #endif #endif /* SINGLE_MCU */ /************* Sensors initialization ***************/ #ifdef USE_INFRARED ir_init(); #endif #ifdef USE_GYRO gyro_init(); #endif #ifdef USE_GPS gps_init(); #endif #ifdef USE_UART0 Uart0Init(); #endif #ifdef USE_UART1 Uart1Init(); #endif #ifdef USE_UART2 Uart2Init(); #endif #ifdef USE_UART3 Uart3Init(); #endif #ifdef USE_USB_SERIAL VCOM_init(); #endif #ifdef USE_GPIO GpioInit(); #endif #ifdef USE_I2C0 i2c0_init(); #endif #ifdef USE_I2C1 i2c1_init(); #endif #ifdef USE_I2C2 i2c2_init(); #endif /************* Links initialization ***************/ #if defined USE_SPI spi_init(); #endif #if defined MCU_SPI_LINK link_mcu_init(); #endif #ifdef MODEM modem_init(); #endif /************ Internal status ***************/ h_ctl_init(); v_ctl_init(); estimator_init(); #ifdef ALT_KALMAN alt_kalman_init(); #endif nav_init(); modules_init(); /** - start interrupt task */ int_enable(); /** wait 0.5s (historical :-) */ sys_time_usleep(500000); #if defined GPS_CONFIGURE gps_configure_uart(); #endif #if defined DATALINK #if DATALINK == XBEE xbee_init(); #endif #endif /* DATALINK */ #if defined AEROCOMM_DATA_PIN IO0DIR |= _BV(AEROCOMM_DATA_PIN); IO0SET = _BV(AEROCOMM_DATA_PIN); #endif power_switch = FALSE; /************ Multi-uavs status ***************/ #ifdef TRAFFIC_INFO traffic_info_init(); #endif }