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++;
}
