int main(void) { mcu_init(); sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); /* Init GPIO for rx pins */ gpio_setup_input_pullup(A_RX_PORT, A_RX_PIN); gpio_setup_input_pullup(B_RX_PORT, B_RX_PIN); /* Init GPIO for tx pins */ gpio_setup_output(A_TX_PORT, A_TX_PIN); gpio_setup_output(B_TX_PORT, B_TX_PIN); gpio_clear(A_TX_PORT, A_TX_PIN); /* */ while (1) { if (sys_time_check_and_ack_timer(0)) { main_periodic(); } main_event(); } return 0; }
void gps_init(void) { multi_gps_mode = MULTI_GPS_MODE; gps.valid_fields = 0; gps.fix = GPS_FIX_NONE; gps.week = 0; gps.tow = 0; gps.cacc = 0; gps.last_3dfix_ticks = 0; gps.last_3dfix_time = 0; gps.last_msg_ticks = 0; gps.last_msg_time = 0; #ifdef GPS_POWER_GPIO gpio_setup_output(GPS_POWER_GPIO); GPS_POWER_GPIO_ON(GPS_POWER_GPIO); #endif #ifdef GPS_LED LED_OFF(GPS_LED); #endif AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS, send_gps); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_INT, send_gps_int); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_LLA, send_gps_lla); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_SOL, send_gps_sol); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SVINFO, send_svinfo); #endif }
/** * Initializing the cyrf chip */ void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, const uint16_t rst_pin) { /* Set spi_peripheral and the status */ cyrf->spi_p = spi_p; cyrf->status = CYRF6936_UNINIT; cyrf->has_irq = FALSE; /* Set the spi transaction */ cyrf->spi_t.cpol = SPICpolIdleLow; cyrf->spi_t.cpha = SPICphaEdge1; cyrf->spi_t.dss = SPIDss8bit; cyrf->spi_t.bitorder = SPIMSBFirst; cyrf->spi_t.cdiv = SPIDiv64; cyrf->spi_t.input_length = 0; cyrf->spi_t.output_length = 0; cyrf->spi_t.input_buf = cyrf->input_buf; cyrf->spi_t.output_buf = cyrf->output_buf; cyrf->spi_t.slave_idx = slave_idx; cyrf->spi_t.select = SPISelectUnselect; cyrf->spi_t.status = SPITransDone; /* Reset the CYRF6936 chip (busy waiting) */ gpio_setup_output(rst_port, rst_pin); gpio_set(rst_port, rst_pin); sys_time_usleep(100); gpio_clear(rst_port, rst_pin); sys_time_usleep(100); /* Get the MFG ID */ cyrf->status = CYRF6936_GET_MFG_ID; cyrf->buffer_idx = 0; cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0xFF); }
void autopilot_init(void) { pprz_mode = PPRZ_MODE_AUTO2; kill_throttle = FALSE; launch = FALSE; autopilot_flight_time = 0; lateral_mode = LATERAL_MODE_MANUAL; gps_lost = FALSE; power_switch = FALSE; #ifdef POWER_SWITCH_GPIO gpio_setup_output(POWER_SWITCH_GPIO); gpio_clear(POWER_SWITCH_GPIO); #endif /* register some periodic message */ register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive); register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", autopilot_send_mode); register_periodic_telemetry(DefaultPeriodic, "DOWNLINK", send_downlink); register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude); register_periodic_telemetry(DefaultPeriodic, "ESTIMATOR", send_estimator); register_periodic_telemetry(DefaultPeriodic, "AIRSPEED", send_airspeed); register_periodic_telemetry(DefaultPeriodic, "BAT", send_bat); register_periodic_telemetry(DefaultPeriodic, "ENERGY", send_energy); register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value); register_periodic_telemetry(DefaultPeriodic, "DESIRED", send_desired); #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS register_periodic_telemetry(DefaultPeriodic, "RC_SETTINGS", send_rc_settings); #endif }
void imu_impl_init(void) { /* Set accel configuration */ adxl345_i2c_init(&imu_aspirin.acc_adxl, &(ASPIRIN_I2C_DEV), ADXL345_ADDR); // set the data rate imu_aspirin.acc_adxl.config.rate = ASPIRIN_ACCEL_RATE; /// @todo drdy int handling for adxl345 //imu_aspirin.acc_adxl.config.drdy_int_enable = TRUE; // With CS tied high to VDD I/O, the ADXL345 is in I2C mode #ifdef ASPIRIN_I2C_CS_PORT gpio_setup_output(ASPIRIN_I2C_CS_PORT, ASPIRIN_I2C_CS_PIN); gpio_set(ASPIRIN_I2C_CS_PORT, ASPIRIN_I2C_CS_PIN); #endif /* Gyro configuration and initalization */ itg3200_init(&imu_aspirin.gyro_itg, &(ASPIRIN_I2C_DEV), ITG3200_ADDR); /* change the default config */ // Aspirin sample rate divider imu_aspirin.gyro_itg.config.smplrt_div = ASPIRIN_GYRO_SMPLRT_DIV; // digital low pass filter imu_aspirin.gyro_itg.config.dlpf_cfg = ASPIRIN_GYRO_LOWPASS; /// @todo eoc interrupt for itg3200, polling for now (including status reg) /* interrupt on data ready, idle high, latch until read any register */ //itg_conf.int_cfg = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7); /* initialize mag and set default options */ hmc58xx_init(&imu_aspirin.mag_hmc, &(ASPIRIN_I2C_DEV), HMC58XX_ADDR); #ifdef IMU_ASPIRIN_VERSION_1_0 imu_aspirin.mag_hmc.type = HMC_TYPE_5843; #endif }
void gps_init(void) { gps.fix = GPS_FIX_NONE; gps.week = 0; gps.tow = 0; gps.cacc = 0; gps.last_3dfix_ticks = 0; gps.last_3dfix_time = 0; gps.last_msg_ticks = 0; gps.last_msg_time = 0; #ifdef GPS_POWER_GPIO gpio_setup_output(GPS_POWER_GPIO); GPS_POWER_GPIO_ON(GPS_POWER_GPIO); #endif #ifdef GPS_LED LED_OFF(GPS_LED); #endif #ifdef GPS_TYPE_H gps_impl_init(); #endif #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "GPS", send_gps); register_periodic_telemetry(DefaultPeriodic, "GPS_INT", send_gps_int); register_periodic_telemetry(DefaultPeriodic, "GPS_LLA", send_gps_lla); register_periodic_telemetry(DefaultPeriodic, "GPS_SOL", send_gps_sol); register_periodic_telemetry(DefaultPeriodic, "SVINFO", send_svinfo); #endif }
void init_mf_daq(void) { mf_daq.nb = 0; mf_daq.power = MF_DAQ_POWER_INIT; #if (defined MF_DAQ_POWER_PORT) && (defined MF_DAQ_POWER_PIN) gpio_setup_output(MF_DAQ_POWER_PORT, MF_DAQ_POWER_PIN); #endif meteo_france_DAQ_SetPower(mf_daq.power); log_started = FALSE; }
void autopilot_init(void) { autopilot_motors_on = false; kill_throttle = ! autopilot_motors_on; autopilot_in_flight = false; autopilot_in_flight_counter = 0; autopilot_mode_auto2 = MODE_AUTO2; autopilot_ground_detected = false; autopilot_detect_ground_once = false; autopilot_flight_time = 0; autopilot_rc = true; autopilot_power_switch = false; #ifdef POWER_SWITCH_GPIO gpio_setup_output(POWER_SWITCH_GPIO); gpio_clear(POWER_SWITCH_GPIO); // POWER OFF #endif // init GNC stack // TODO this should be done in modules init nav_init(); guidance_h_init(); guidance_v_init(); stabilization_init(); stabilization_none_init(); #if USE_STABILIZATION_RATE stabilization_rate_init(); #endif stabilization_attitude_init(); // call implementation init // it will set startup mode #if USE_GENERATED_AUTOPILOT autopilot_generated_init(); #else autopilot_static_init(); #endif // register messages register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ATTITUDE, send_attitude); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ENERGY, send_energy); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP_MIN, send_fp_min); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_CMD, send_rotorcraft_cmd); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DL_VALUE, send_dl_value); #ifdef ACTUATORS register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators); #endif #ifdef RADIO_CONTROL register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc); #endif }
void humid_sht_init(void) { /* Configure DAT/SCL pin as GPIO */ gpio_setup_input(SHT_DAT_GPIO); gpio_setup_output(SHT_SCK_GPIO); gpio_clear(SHT_SCK_GPIO); humid_sht_available = FALSE; humid_sht_status = SHT_IDLE; }
void bluegiga_init(struct bluegiga_periph *p) { #ifdef MODEM_LED LED_INIT(MODEM_LED); #endif // configure the SPI bus bluegiga_spi.input_buf = p->work_rx; bluegiga_spi.output_buf = p->work_tx; bluegiga_spi.input_length = 20; bluegiga_spi.output_length = 20; bluegiga_spi.slave_idx = 0; // Not used for SPI-Slave: always NSS pin bluegiga_spi.select = SPISelectUnselect; bluegiga_spi.cpol = SPICpolIdleHigh; bluegiga_spi.cpha = SPICphaEdge2; bluegiga_spi.dss = SPIDss8bit; bluegiga_spi.bitorder = SPIMSBFirst; bluegiga_spi.cdiv = SPIDiv64; bluegiga_spi.after_cb = (SPICallback) trans_cb; // Configure generic link device p->device.periph = (void *)(p); p->device.check_free_space = (check_free_space_t) dev_check_free_space; p->device.put_byte = (put_byte_t) dev_put_byte; p->device.send_message = (send_message_t) dev_send_message; p->device.char_available = (char_available_t) dev_char_available; p->device.get_byte = (get_byte_t) dev_get_byte; // initialize peripheral variables p->rx_insert_idx = 0; p->rx_extract_idx = 0; p->tx_insert_idx = 0; p->tx_extract_idx = 0; for (int i = 0; i < bluegiga_spi.input_length; i++) { p->work_rx[i] = 0; } for (int i = 0; i < bluegiga_spi.output_length; i++) { p->work_tx[i] = 0; } for (int i = 0; i < 255; i++) { bluegiga_rssi[i] = 127; } // set DRDY interrupt pin for spi master triggered on falling edge gpio_setup_output(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // register spi slave read for transaction spi_slave_register(&(BLUEGIGA_SPI_DEV), &bluegiga_spi); coms_status = BLUEGIGA_UNINIT; }
void spi_init_slaves(void) { #if USE_SPI_SLAVE0 gpio_setup_output(SPI_SELECT_SLAVE0_PORT, SPI_SELECT_SLAVE0_PIN); SpiSlaveUnselect(0); #endif #if USE_SPI_SLAVE1 gpio_setup_output(SPI_SELECT_SLAVE1_PORT, SPI_SELECT_SLAVE1_PIN); SpiSlaveUnselect(1); #endif #if USE_SPI_SLAVE2 gpio_setup_output(SPI_SELECT_SLAVE2_PORT, SPI_SELECT_SLAVE2_PIN); SpiSlaveUnselect(2); #endif #if USE_SPI_SLAVE3 gpio_setup_output(SPI_SELECT_SLAVE3_PORT, SPI_SELECT_SLAVE3_PIN); SpiSlaveUnselect(3); #endif #if USE_SPI_SLAVE4 gpio_setup_output(SPI_SELECT_SLAVE4_PORT, SPI_SELECT_SLAVE4_PIN); SpiSlaveUnselect(4); #endif #if USE_SPI_SLAVE5 gpio_setup_output(SPI_SELECT_SLAVE5_PORT, SPI_SELECT_SLAVE5_PIN); SpiSlaveUnselect(5); #endif }
void imu_init(void) { #ifdef IMU_POWER_GPIO gpio_setup_output(IMU_POWER_GPIO); IMU_POWER_GPIO_ON(IMU_POWER_GPIO); #endif /* initialises neutrals */ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); #if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); #else #if USE_MAGNETOMETER INFO("Magnetometer neutrals are set to zero, you should calibrate!") #endif INT_VECT3_ZERO(imu.mag_neutral); #endif /* Compute quaternion and rotation matrix for conversions between body and imu frame */ struct Int32Eulers body_to_imu_eulers = { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALIZE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); #if USE_IMU_FLOAT #else // !USE_IMU_FLOAT register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag); #endif // !USE_IMU_FLOAT #endif // DOWNLINK imu_impl_init(); }
void autopilot_init(void) { /* mode is finally set at end of init if MODE_STARTUP is not KILL */ autopilot_mode = AP_MODE_KILL; autopilot_motors_on = false; kill_throttle = ! autopilot_motors_on; autopilot_in_flight = false; autopilot_in_flight_counter = 0; autopilot_mode_auto2 = MODE_AUTO2; autopilot_ground_detected = false; autopilot_detect_ground_once = false; autopilot_flight_time = 0; autopilot_rc = true; autopilot_power_switch = false; #ifdef POWER_SWITCH_GPIO gpio_setup_output(POWER_SWITCH_GPIO); gpio_clear(POWER_SWITCH_GPIO); // POWER OFF #endif autopilot_arming_init(); nav_init(); guidance_h_init(); guidance_v_init(); stabilization_init(); stabilization_none_init(); #if USE_STABILIZATION_RATE stabilization_rate_init(); #endif stabilization_attitude_init(); /* set startup mode, propagates through to guidance h/v */ autopilot_set_mode(MODE_STARTUP); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ATTITUDE, send_attitude); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ENERGY, send_energy); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP_MIN, send_fp_min); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_CMD, send_rotorcraft_cmd); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DL_VALUE, send_dl_value); #ifdef ACTUATORS register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators); #endif #ifdef RADIO_CONTROL register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc); #endif }
// Init function void radio_control_impl_init(void) { sbus.frame_available = FALSE; sbus.status = SBUS_STATUS_UNINIT; // Set UART parameters (100K, 8 bits, 2 stops, even parity) uart_periph_set_bits_stop_parity(&SBUS_UART_DEV, UBITS_8, USTOP_2, UPARITY_EVEN); uart_periph_set_baudrate(&SBUS_UART_DEV, B100000); // Set polarity #ifdef RC_POLARITY_GPIO_PORT gpio_setup_output(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); RC_SET_POLARITY(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); #endif }
int main(void) { // not calling mcu_init with PERIPHERALS_AUTO_INIT // rather explicitly init only sys_time mcu_arch_init(); sys_time_init(); gpio_setup_output(TEST_GPIO1); gpio_setup_output(TEST_GPIO2); unsigned int tmr_2 = sys_time_register_timer(2, NULL); sys_time_register_timer(1, main_periodic); mcu_int_enable(); while(1) { if (sys_time_check_and_ack_timer(tmr_2)) { main_periodic_2(); } } return 0; }
void autopilot_init(void) { /* mode is finally set at end of init if MODE_STARTUP is not KILL */ autopilot_mode = AP_MODE_KILL; autopilot_motors_on = FALSE; kill_throttle = ! autopilot_motors_on; autopilot_in_flight = FALSE; autopilot_in_flight_counter = 0; autopilot_mode_auto2 = MODE_AUTO2; autopilot_ground_detected = FALSE; autopilot_detect_ground_once = FALSE; autopilot_flight_time = 0; autopilot_rc = TRUE; autopilot_power_switch = FALSE; #ifdef POWER_SWITCH_GPIO gpio_setup_output(POWER_SWITCH_GPIO); gpio_clear(POWER_SWITCH_GPIO); // POWER OFF #endif autopilot_arming_init(); nav_init(); guidance_h_init(); guidance_v_init(); stabilization_init(); stabilization_none_init(); stabilization_rate_init(); stabilization_attitude_init(); /* set startup mode, propagates through to guidance h/v */ autopilot_set_mode(MODE_STARTUP); register_periodic_telemetry(DefaultPeriodic, "AUTOPILOT_VERSION", send_autopilot_version); register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive); register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_STATUS", send_status); register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude); register_periodic_telemetry(DefaultPeriodic, "ENERGY", send_energy); register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_FP", send_fp); register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CMD", send_rotorcraft_cmd); register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value); #ifdef ACTUATORS register_periodic_telemetry(DefaultPeriodic, "ACTUATORS", send_actuators); #endif #ifdef RADIO_CONTROL register_periodic_telemetry(DefaultPeriodic, "RC", send_rc); register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_RADIO_CONTROL", send_rotorcraft_rc); #endif }
void humid_sht_init(void) { /* Configure DAT/SCL pin as GPIO */ gpio_setup_input(SHT_DAT_GPIO); gpio_setup_output(SHT_SCK_GPIO); gpio_clear(SHT_SCK_GPIO); humid_sht_available = false; humid_sht_status = SHT_IDLE; #if SHT_SDLOG log_sht_started = false; #endif }
void rotorcraft_cam_init(void) { #ifdef ROTORCRAFT_CAM_SWITCH_GPIO gpio_setup_output(ROTORCRAFT_CAM_SWITCH_GPIO); #endif rotorcraft_cam_SetCamMode(ROTORCRAFT_CAM_DEFAULT_MODE); #if ROTORCRAFT_CAM_USE_TILT rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL; ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm); #else rotorcraft_cam_tilt_pwm = 1500; #endif rotorcraft_cam_tilt = 0; rotorcraft_cam_pan = 0; register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CAM", send_cam); }
void sbus_common_init(struct Sbus *sbus_p, struct uart_periph *dev) { sbus_p->frame_available = false; sbus_p->status = SBUS_STATUS_UNINIT; // Set UART parameters (100K, 8 bits, 2 stops, even parity) uart_periph_set_bits_stop_parity(dev, UBITS_8, USTOP_2, UPARITY_EVEN); uart_periph_set_baudrate(dev, B100000); // Set polarity #ifdef RC_POLARITY_GPIO_PORT gpio_setup_output(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); RC_SET_POLARITY(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); #endif }
void imu_init(void) { #ifdef IMU_POWER_GPIO gpio_setup_output(IMU_POWER_GPIO); IMU_POWER_GPIO_ON(IMU_POWER_GPIO); #endif /* initialises neutrals */ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); #if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); #else #if USE_MAGNETOMETER INFO("Magnetometer neutrals are set to zero, you should calibrate!") #endif INT_VECT3_ZERO(imu.mag_neutral); #endif struct FloatEulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); #if USE_IMU_FLOAT orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); #endif #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); #if !USE_IMU_FLOAT register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag); #endif // !USE_IMU_FLOAT #endif // DOWNLINK imu_impl_init(); }
void mf_ptu_init(void) { adc_buf_channel(ADC_CHANNEL_PRESSURE, &pressure_buf, ADC_CHANNEL_PTU_NB_SAMPLES); adc_buf_channel(ADC_CHANNEL_TEMPERATURE, &temp_buf, ADC_CHANNEL_PTU_NB_SAMPLES); #ifdef PTU_POWER_GPIO gpio_setup_output(PTU_POWER_GPIO); gpio_set(PTU_POWER_GPIO); #endif pressure_adc = 0; temp_adc = 0; humid_period = 0; #if LOG_PTU log_ptu_started = false; #endif }
void gps_init(void) { multi_gps_mode = MULTI_GPS_MODE; gps.valid_fields = 0; gps.fix = GPS_FIX_NONE; gps.week = 0; gps.tow = 0; gps.cacc = 0; gps.last_3dfix_ticks = 0; gps.last_3dfix_time = 0; gps.last_msg_ticks = 0; gps.last_msg_time = 0; #ifdef GPS_POWER_GPIO gpio_setup_output(GPS_POWER_GPIO); GPS_POWER_GPIO_ON(GPS_POWER_GPIO); #endif #ifdef GPS_LED LED_OFF(GPS_LED); #endif RegisterGps(PRIMARY_GPS); #ifdef SECONDARY_GPS RegisterGps(SECONDARY_GPS); #endif for (int i=0; i < GPS_NB_IMPL; i++) { if (GpsInstances[i].init != NULL) { GpsInstances[i].init(); } } AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS, send_gps); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_INT, send_gps_int); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_LLA, send_gps_lla); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_SOL, send_gps_sol); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SVINFO, send_svinfo); #endif }
void sbus_common_init(struct Sbus *sbus_p, struct uart_periph *dev, gpio_port_t gpio_polarity_port, uint16_t gpio_polarity_pin) { sbus_p->frame_available = false; sbus_p->status = SBUS_STATUS_UNINIT; // Set UART parameters (100K, 8 bits, 2 stops, even parity) uart_periph_set_baudrate(dev, B100000); uart_periph_set_bits_stop_parity(dev, UBITS_8, USTOP_2, UPARITY_EVEN); // Try to invert RX data logic when available in hardware periph uart_periph_invert_data_logic(dev, true, false); // Set polarity (when not done in hardware, don't use both!) if (gpio_polarity_port != 0) { gpio_setup_output(gpio_polarity_port, gpio_polarity_pin); RC_SET_POLARITY(gpio_polarity_port, gpio_polarity_pin); } }
// Init function void radio_control_impl_init(void) { sbus.frame_available = FALSE; sbus.status = SBUS_STATUS_UNINIT; // Set UART parameters (100K, 8 bits, 2 stops, even parity) uart_periph_set_bits_stop_parity(&SBUS_UART_DEV, UBITS_8, USTOP_2, UPARITY_EVEN); uart_periph_set_baudrate(&SBUS_UART_DEV, B100000); // Set polarity #ifdef RC_POLARITY_GPIO_PORT gpio_setup_output(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); RC_SET_POLARITY(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); #endif // Register telemetry message #if PERIODIC_TELEMETRY register_periodic_telemetry(DOWNLINK_TELEMETRY, "PPM", send_sbus); #endif }
void w5100_init( void ) { // configure the SPI bus. w5100_spi.slave_idx = W5100_SLAVE_IDX; w5100_spi.output_length = 4; w5100_spi.input_length = 4; w5100_spi.select = SPISelectUnselect; w5100_spi.cpol = SPICpolIdleLow; w5100_spi.cpha = SPICphaEdge1; w5100_spi.dss = SPIDss8bit; w5100_spi.bitorder = SPIMSBFirst; w5100_spi.cdiv = SPIDiv64; chip0.status = W5100StatusUninit; chip0.curbuf = 0; w5100_spi.input_buf = &chip0.work_rx[0]; w5100_spi.output_buf = &chip0.work_tx[0]; // wait one second for proper initialization (chip getting powered up). sys_time_usleep(1000000); // set DRDY pin gpio_setup_output(W5100_DRDY_GPIO, W5100_DRDY_GPIO_PIN); gpio_clear(W5100_DRDY_GPIO, W5100_DRDY_GPIO_PIN); sys_time_usleep(200); gpio_set(W5100_DRDY_GPIO, W5100_DRDY_GPIO_PIN); // allow some time for the chip to wake up. sys_time_usleep(20000); // write reset bit into mode register w5100_set( REG_MR, 1<<RST ); // allow some time to wake up... sys_time_usleep(20000); // receive memory size w5100_set( REG_RX_MEM, 0x55 ); // transmit memory size w5100_set( REG_TX_MEM, 0x55 ); // Setting the required socket base addresses for reads and writes to/from sockets for (int i=0; i<SOCKETS; i++) { SBASE[i] = TXBUF_BASE + SSIZE * i; RBASE[i] = RXBUF_BASE + RSIZE * i; } uint8_t gateway[4]; gateway[0] = ip[0]; gateway[1] = ip[1]; gateway[2] = ip[2]; gateway[3] = 1; // configure gateway, subnet, mac and ip on "NIC". w5100_set_buffer( REG_GAR, gateway, 4 ); w5100_set_buffer( REG_SUBR, subnet, 4 ); w5100_set_buffer( REG_SHAR, mac, 6 ); w5100_set_buffer( REG_SIPR, ip, 4 ); // create a socket to send telemetry through. configure_socket( TELEM_SOCKET, SNMR_MULTI, 1, dport, dest ); // make dest zero and configure socket to receive data dest[ 0 ] = 0x00; configure_socket( CMD_SOCKET, 0, dport, dport, dest ); }
/** * Initialize the navdata board */ bool navdata_init() { assert(sizeof(struct navdata_measure_t) == NAVDATA_PACKET_SIZE); /* Check if the FD isn't already initialized */ if (navdata.fd <= 0) { navdata.fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY); /* O_NONBLOCK doesn't work */ if (navdata.fd < 0) { printf("[navdata] Unable to open navdata board connection(/dev/ttyO1)\n"); return false; } /* Update the settings of the UART connection */ fcntl(navdata.fd, F_SETFL, 0); /* read calls are non blocking */ /* set port options */ struct termios options; /* Get the current options for the port */ tcgetattr(navdata.fd, &options); /* Set the baud rates to 460800 */ cfsetispeed(&options, B460800); cfsetospeed(&options, B460800); options.c_cflag |= (CLOCAL | CREAD); /* Enable the receiver and set local mode */ options.c_iflag = 0; /* clear input options */ options.c_lflag = 0; /* clear local options */ options.c_oflag &= ~OPOST; //clear output options (raw output) //Set the new options for the port tcsetattr(navdata.fd, TCSANOW, &options); } // Reset available flags navdata_available = false; navdata.baro_calibrated = false; navdata.baro_available = false; navdata.imu_lost = false; // Set all statistics to 0 navdata.checksum_errors = 0; navdata.lost_imu_frames = 0; navdata.totalBytesRead = 0; navdata.packetsRead = 0; navdata.last_packet_number = 0; /* Stop acquisition */ navdata_cmd_send(NAVDATA_CMD_STOP); /* Read the baro calibration(blocking) */ if (!navdata_baro_calib()) { printf("[navdata] Could not acquire baro calibration!\n"); return false; } navdata.baro_calibrated = true; /* Start acquisition */ navdata_cmd_send(NAVDATA_CMD_START); /* Set navboard gpio control */ gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); /* Start navdata reading thread */ pthread_t navdata_thread; if (pthread_create(&navdata_thread, NULL, navdata_read, NULL) != 0) { printf("[navdata] Could not create navdata reading thread!\n"); return false; } #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ARDRONE_NAVDATA, send_navdata); #endif /* Set to initialized */ navdata.is_initialized = true; return true; }
static void i2c_wd_check(struct i2c_periph *periph) { uint32_t i2c = (uint32_t) periph->reg_addr; if (periph->watchdog > WD_DELAY) { if (periph->watchdog == WD_DELAY + 1) { i2c_disable_interrupt(i2c, I2C_CR2_ITEVTEN); i2c_disable_interrupt(i2c, I2C_CR2_ITERREN); i2c_peripheral_disable(i2c); #if USE_I2C1 if (i2c == I2C1) { gpio_setup_output(I2C1_GPIO_PORT, I2C1_GPIO_SCL); gpio_setup_input(I2C1_GPIO_PORT, I2C1_GPIO_SDA); } #endif #if USE_I2C2 if (i2c == I2C2) { gpio_setup_output(I2C2_GPIO_PORT, I2C2_GPIO_SCL); gpio_setup_input(I2C2_GPIO_PORT, I2C2_GPIO_SDA); } #endif #if USE_I2C3 if (i2c == I2C3) { gpio_setup_output(I2C3_GPIO_PORT_SCL, I2C3_GPIO_SCL); gpio_setup_input(I2C3_GPIO_PORT_SDA,I2C3_GPIO_SDA); } #endif i2c_scl_clear(i2c); } else if (periph->watchdog < WD_DELAY + WD_RECOVERY_TICKS) { if ((periph->watchdog - WD_DELAY) % 2) i2c_scl_clear(i2c); else i2c_scl_set(i2c); } else { i2c_scl_set(i2c); /* setup gpios for normal i2c operation again */ i2c_setup_gpio(i2c); periph->trans_insert_idx = 0; periph->trans_extract_idx = 0; periph->status = I2CIdle; i2c_enable_interrupt(i2c, I2C_CR2_ITEVTEN); i2c_enable_interrupt(i2c, I2C_CR2_ITERREN); i2c_peripheral_enable(i2c); periph->watchdog = 0; // restart watchdog periph->errors->timeout_tlow_cnt++; return; } } if (periph->watchdog >= 0) periph->watchdog++; }