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 }
STATIC_INLINE void main_init(void) { mcu_init(); electrical_init(); stateInit(); actuators_init(); #if USE_MOTOR_MIXING motor_mixing_init(); #endif radio_control_init(); #if USE_BARO_BOARD baro_init(); #endif #if USE_IMU imu_init(); #endif #if USE_AHRS_ALIGNER ahrs_aligner_init(); #endif #if USE_AHRS ahrs_init(); #endif ins_init(); #if USE_GPS gps_init(); #endif autopilot_init(); modules_init(); settings_init(); mcu_int_enable(); #if DOWNLINK downlink_init(); #endif // register the timers for the periodic functions main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL); radio_control_tid = sys_time_register_timer((1. / 60.), NULL); failsafe_tid = sys_time_register_timer(0.05, NULL); electrical_tid = sys_time_register_timer(0.1, NULL); telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL); #if USE_BARO_BOARD baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL); #endif #if USE_IMU // send body_to_imu from here for now AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu)); #endif // Do a failsafe check first failsafe_check(); }
void init_ap( void ) { #ifndef SINGLE_MCU /** init done in main_fbw in single MCU */ mcu_init(); #endif /* SINGLE_MCU */ /************* Sensors initialization ***************/ #if USE_GPS gps_init(); #endif #ifdef USE_GPIO GpioInit(); #endif #if USE_IMU imu_init(); #endif #if USE_AHRS_ALIGNER ahrs_aligner_init(); #endif #if USE_AHRS ahrs_init(); #endif #if USE_BAROMETER baro_init(); #endif ins_init(); stateInit(); /************* Links initialization ***************/ #if defined MCU_SPI_LINK || defined MCU_UART_LINK link_mcu_init(); #endif #if USE_AUDIO_TELEMETRY audio_telemetry_init(); #endif /************ Internal status ***************/ autopilot_init(); h_ctl_init(); v_ctl_init(); nav_init(); modules_init(); settings_init(); /**** start timers for periodic functions *****/ sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL); navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL); attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL); modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL); telemetry_tid = sys_time_register_timer(1./60, NULL); monitor_tid = sys_time_register_timer(1.0, NULL); /** - start interrupt task */ mcu_int_enable(); #if defined DATALINK #if DATALINK == XBEE xbee_init(); #endif #if DATALINK == W5100 w5100_init(); #endif #endif /* DATALINK */ #if defined AEROCOMM_DATA_PIN IO0DIR |= _BV(AEROCOMM_DATA_PIN); IO0SET = _BV(AEROCOMM_DATA_PIN); #endif /************ Multi-uavs status ***************/ #ifdef TRAFFIC_INFO traffic_info_init(); #endif #ifdef SET_IMU_ZERO_ON_STARTUP #ifndef SITL //wait 10secs for init sys_time_usleep(10000000); imu_store_bias(); #endif #endif }
void init_ap( void ) { #ifndef SINGLE_MCU /** init done in main_fbw in single MCU */ mcu_init(); #endif /* SINGLE_MCU */ /****** initialize and reset state interface ********/ stateInit(); /************* Sensors initialization ***************/ #if USE_GPS gps_init(); #endif #if USE_IMU imu_init(); #if USE_IMU_FLOAT imu_float_init(); #endif #endif #if USE_AHRS_ALIGNER ahrs_aligner_init(); #endif #if USE_AHRS ahrs_init(); #endif #if USE_AHRS && USE_IMU register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_fliter_status); #endif air_data_init(); #if USE_BARO_BOARD baro_init(); #endif ins_init(); /************* Links initialization ***************/ #if defined MCU_SPI_LINK || defined MCU_UART_LINK link_mcu_init(); #endif #if USE_AUDIO_TELEMETRY audio_telemetry_init(); #endif /************ Internal status ***************/ autopilot_init(); h_ctl_init(); v_ctl_init(); nav_init(); modules_init(); settings_init(); /**** start timers for periodic functions *****/ sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL); navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL); attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL); modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL); telemetry_tid = sys_time_register_timer(1./TELEMETRY_FREQUENCY, NULL); monitor_tid = sys_time_register_timer(1.0, NULL); /** - start interrupt task */ mcu_int_enable(); #if defined DATALINK #if DATALINK == XBEE xbee_init(); #endif #if DATALINK == W5100 w5100_init(); #endif #endif /* DATALINK */ #if defined AEROCOMM_DATA_PIN IO0DIR |= _BV(AEROCOMM_DATA_PIN); IO0SET = _BV(AEROCOMM_DATA_PIN); #endif /************ Multi-uavs status ***************/ #ifdef TRAFFIC_INFO traffic_info_init(); #endif /* set initial trim values. * these are passed to fbw via inter_mcu. */ ap_state->command_roll_trim = COMMAND_ROLL_TRIM; ap_state->command_pitch_trim = COMMAND_PITCH_TRIM; ap_state->command_yaw_trim = COMMAND_YAW_TRIM; }
int main() { //uint32_t count; MSS_GPIO_init(); MSS_GPIO_config( MSS_GPIO_0, MSS_GPIO_OUTPUT_MODE); MSS_GPIO_config( MSS_GPIO_1, MSS_GPIO_OUTPUT_MODE); MSS_I2C_init(&g_mss_i2c1 , IMU_ADDRESS_WRITE, MSS_I2C_PCLK_DIV_256 ); imu_init(); //one timer tick is 10ns at 100Mhz MSS_TIM1_init( MSS_TIMER_PERIODIC_MODE); MSS_TIM1_enable_irq(); MSS_TIM1_load_background(100000); MSS_TIM1_start(); MSS_TIM2_init( MSS_TIMER_ONE_SHOT_MODE); while( 1 ) { //int16_t x_accl = imu_accl_x(); //int16_t y_accl = imu_accl_y(); //int16_t z_accl = imu_accl_z(); X_GYRO = imu_gyro_x(); Y_ACCL = imu_accl_y(); //uint16_t y_gyro = imu_gyro_y(); //uint16_t z_gyro = imu_gyro_z(); //printf("Avg X: %f\r\n", avg_x); //printf("Accel X: %i\n\r", x_accl); //printf("Accel Y: %u\n\r", y_accl); //printf("Accel Z: %u\n\r", z_accl); //printf("Gyro X: %u\r\n", x_gyro); //printf("Gyro Y: %u\r\n", y_gyro); //printf("Gyro Z: %u\r\n", z_gyro); //150000 = 1.5ms pulse //1.49ms pulse = equilibrium point uint32_t pulsewidth_right = 150000 + (angle/90.0)*100000; uint32_t pulsewidth_left = 150000 - (angle/90.0)*100000; uint32_t period = 2000000; MSS_TIM2_load_immediate(period); MSS_TIM2_start(); while (MSS_TIM2_get_current_value() > 0) { if (MSS_TIM2_get_current_value() > (period - pulsewidth_right)) MSS_GPIO_set_output( MSS_GPIO_0, 1); else MSS_GPIO_set_output( MSS_GPIO_0, 0); if (MSS_TIM2_get_current_value() > (period - pulsewidth_left)) MSS_GPIO_set_output( MSS_GPIO_1, 1); else MSS_GPIO_set_output( MSS_GPIO_1, 0); } } return 0; }
int main (void) { unsigned char value; //Configure all ports as inputs TRISA = 0xFFFF; TRISB = 0xFFFF;// TRISC = 0xFFFF; //--------------------------------------------------------------------- // OSCILATOR //--------------------------------------------------------------------- oscilator_init(); //config_load(); //--------------------------------------------------------------------- // UART //--------------------------------------------------------------------- uart_init(); printf("Uart Init Ok\n"); //--------------------------------------------------------------------- // STATUS (LED/SOUND) //--------------------------------------------------------------------- status_init(); // STATUS(BLINK_SLOW,_ON,_ON); // __delay_ms(10); // STATUS_INIT; //--------------------------------------------------------------------- // ADC //--------------------------------------------------------------------- // adc_init(); // timer_init_ms(); //--------------------------------------------------------------------- // I2C //--------------------------------------------------------------------- i2c_init(); printf("i2c Init Ok\n"); ITG3200_begin(); printf("ITG3200 Ok\n"); ADXL345_begin(); printf("ADXL Init Ok\n"); //STATUS(BLINK_FAST,_ON,_ON); printf("Init Gyro\n"); // STATUS_NORMAL; //__delay_ms(100); // Acceleromter calibration int AcclCalibration; AcclCalibration =1; //#define AccCali #ifdef AccCali while(AcclCalibration){ int i; double adjAccl; for(i=0;i<10;i++){ ADXL345_update(); } float Acclx=getAcclXOutput(); float Accly=getAcclYOutput(); float Acclz=getAcclZOutput(); float Gravity=sqrt(Acclx*Acclx+Accly*Accly+Acclz*Acclz); adjAccl= 1/Gravity; ADXL345_Scaler =adjAccl * ADXL345_Scaler; printf("%1.3f %1.3f %1.3f Gravity %1.9f y %1.9f %1.3f\n",Acclx,Accly,Acclz,Gravity,ADXL345_Scaler,adjAccl); if (Gravity == 1.0000) AcclCalibration=0; } printf("Accelerometer Calibrated \n"); #endif //#define GyroTest #ifdef GyroTest while(1){ ITG3200_update(); float GyroX=getGyroXOutput(); float GyroY=getGyroYOutput(); float GyroZ=getGyroZOutput(); int ID=getGyroIDOutput(); float Temp= getGyroTempOutput(); printf("%1.3f %1.3f %1.3f %1.3f %i \n",GyroX,GyroY,GyroZ,Temp,ID); } #endif //--------------------------------------------------------------------- // MOTOR //--------------------------------------------------------------------- motor_init(); printf("motor init ok\n"); __delay_ms(100); //#define motor_debug #ifdef motor_debug //while(1){ // _LAT(pinMotor0) = 1; // _LAT(pinMotor1) = 1; // _LAT(pinMotor2) = 1; // _LAT(pinMotor3) = 1; // // _TRIS(pinMotor0) = 0; // _TRIS(pinMotor1) = 0; // _TRIS(pinMotor2) = 0; // _TRIS(pinMotor3)= 0; // // //} int i=0; while(i<50){ _LAT(pinLed1) =! _LAT(pinLed1); motor_set_duty(0,i); motor_set_duty(1,i); motor_set_duty(2,i); motor_set_duty(3,i); motor_apply_duty(); __delay_ms(50); i = i + 1; // if(i> 90) i = 0; printf("Motor %x \n"); } __delay_ms(250); i=0; motor_set_duty(0,i); motor_apply_duty(); motor_set_duty(1,i); __delay_ms(250); motor_apply_duty(); motor_set_duty(2,i); __delay_ms(250); motor_apply_duty(); motor_set_duty(3,i); __delay_ms(250); motor_apply_duty(); __delay_ms(250); #endif //--------------------------------------------------------------------- // IMU //--------------------------------------------------------------------- imu_init(); //#define imu_debug #ifdef imu_debug //IMU debug comment out for production float interval_ms=0; while (1){ ITG3200_update(); ADXL345_update(); //interval_ms = timer_dt(); // if(interval_ms > 0 ){ //we have fresh adc samples // printf("Time %lu ",interval_us); imu_update(interval_ms); float* K = dcmEst[2]; //K(body) vector from DCM matrix float pitch_roll = acos(K[2]); //total pitch and roll, angle necessary to bring K to [0,0,1] //cos(K,K0) = [Kx,Ky,Kz].[0,0,1] = Kz float Kxy = sqrt(K[0]*K[0] + K[1]*K[1]); float anglePitch = - (pitch_roll * asin(K[0]/Kxy))*50;//*180/PI ; float angleRoll = (pitch_roll * asin(K[1]/Kxy))*50;//*180/PI; //float angleRoll = (atan2(dcmEst[2][2],dcmEst[2][1]))*180/3.14; //float anglePitch = -(asin(dcmEst[2][0]))*180/3.14; // float angleYaw=(-atan2(dcmGyro[1][0],dcmGyro[0][0]))*180/3.14; float driftX=(sin(anglePitch*(3.14/180))*(sin(angleRoll*(3.14/180)))); float CalcDriftX=getAcclXOutput()-driftX; float Acclx=getAcclXOutput(); float Gyrox=getGyroXOutput(); float Accly=getAcclYOutput(); float Acclz=getAcclZOutput(); float Gyroy=getGyroYOutput(); //printf("\n"); //if(0 == imu_sequence % 4){ // hdlc_send_byte(float_to_int(anglePitch)); // hdlc_send_byte(float_to_int(angleRoll)); // hdlc_send_byte(float_to_int(Acclx*100)); // hdlc_send_byte(float_to_int(Gyrox*1000)); // hdlc_send_byte(float_to_int(Accly*100)); // hdlc_send_byte(float_to_int(Gyroy*1000)); // //hdlc_send_byte(float_to_int(Acclz*100)); // //hdlc_send_byte(float_to_int(Kxy*100)); // //hdlc_send_byte(float_to_int(K[0]*100)); // //hdlc_send_byte(float_to_int(K[1]*100)); // //hdlc_send_byte(float_to_int(K[2]*100)); // //// float GyroYpitch = GyroYpitch+ (getGyroYOutput()*(interval_us/1000)); //// printf("pitch %1.3f roll %1.3f Gyro ",pitch.value,roll.value,GyroYpitch); //// printf(" Angle P%1.3f R%1.3f Drift=%1.3f Acclx=%1.3f Calc=%1.3f GyroX=%1.3f\n",anglePitch,angleRoll,driftX,Acclx,CalcDriftX,GyroX); // hdlc_send_sep(); //} } }
int threads_linux_init(void) { int status = 0; int n = 0; int return_value = THREADS_LINUX_SUCCESS; // Init data status = sensors_init(&battery_data, &gps_data, &imu_data, &pitot_data, &pwm_read_data, &pwm_write_data, &scp1000_data, &sonar_data); if(status != SENSORS_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } if(return_value != THREADS_LINUX_FAILURE) { status = calibration_init(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data); if(status != CALIBRATION_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } } if(return_value != THREADS_LINUX_FAILURE) { status = gps_init(&gps_measure); if(status != 1) { return_value = THREADS_LINUX_FAILURE; } } if(return_value != THREADS_LINUX_FAILURE) { status = imu_init(&imu_measure); if(status != 1) { return_value = THREADS_LINUX_FAILURE; } } if(return_value != THREADS_LINUX_FAILURE) { status = magnetometer_init(&magnetometer_measure); if(status != 1) { return_value = THREADS_LINUX_FAILURE; } } if(return_value != THREADS_LINUX_FAILURE) { status = sonar_init(&sonar_measure); if(status != 1) { return_value = THREADS_LINUX_FAILURE; } } if(return_value != THREADS_LINUX_FAILURE) { status = estimation_init(ESTIMATION_DO_NOT_ESTIMATE_ACCEL_BIAS, &estimation_data); if(status != ESTIMATION_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } } if(return_value != THREADS_LINUX_FAILURE) { status = control_init(&control_data); if(status != CONTROL_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } } if(return_value != THREADS_LINUX_FAILURE) { status = protocol_init(); if(status != PROTOCOL_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } } if(return_value != THREADS_LINUX_FAILURE) { status = ui_init(); if(status != UI_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } } if(return_value != THREADS_LINUX_FAILURE) { status = datalogger_init(); if(status != DATALOGGER_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } } // Main Loop if(return_value != THREADS_LINUX_FAILURE) { threads_linux_timer_start_task_1(); usleep(0.5*task_1_period_us); threads_linux_timer_start_task_2(); usleep(5*task_1_period_us); while(quittask == 0) { #if ANU_COMPILE_FOR_OVERO #else if(datalogger_status() == DATALOGGER_RUNNING) datalogger_update_IPC(); #endif if(++n>100) { if(datalogger_status() == DATALOGGER_RUNNING) datalogger_write_file(); n = 0; } usleep(5*task_1_period_us); } threads_linux_timer_stop_task_1(); usleep(TASK1_PERIOD_US); threads_linux_timer_stop_task_2(); usleep(TASK2_PERIOD_US); } status = datalogger_close(); if(status != DATALOGGER_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } status = ui_close(); if(status != UI_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } status = protocol_close(); if(status != PROTOCOL_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } status = control_close(); if(status != CONTROL_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } status = estimation_close(&estimation_data); if(status != ESTIMATION_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } status = magnetometer_close(&magnetometer_measure); if(status != 1) { return_value = THREADS_LINUX_FAILURE; } status = imu_close(&imu_measure); if(status != 1) { return_value = THREADS_LINUX_FAILURE; } status = gps_close(&gps_measure); if(status != 1) { return_value = THREADS_LINUX_FAILURE; } status = calibration_close(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data); if(status != CALIBRATION_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } status = sensors_close(); if(status != SENSORS_SUCCESS) { return_value = THREADS_LINUX_FAILURE; } return return_value; }
void aos_init(int traj_nb) { aos.traj = &traj[traj_nb]; aos.time = 0; aos.dt = 1. / AHRS_PROPAGATE_FREQUENCY; aos.traj->ts = 0; aos.traj->ts = 1.; // default to one second /* default state */ EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler); RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); FLOAT_VECT3_ZERO(aos.ltp_pos); FLOAT_VECT3_ZERO(aos.ltp_vel); FLOAT_VECT3_ZERO(aos.ltp_accel); FLOAT_VECT3_ZERO(aos.ltp_jerk); aos.traj->init_fun(); imu_init(); ahrs_init(); #ifdef PERFECT_SENSORS RATES_ASSIGN(aos.gyro_bias, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.); aos.heading_noise = 0.; #else RATES_ASSIGN(aos.gyro_bias, RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.)); RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.)); VECT3_ASSIGN(aos.accel_noise, .5, .5, .5); aos.heading_noise = RadOfDeg(3.); #endif #ifdef FORCE_ALIGNEMENT // DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat); aos_compute_sensors(); // DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias); // DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates); VECT3_COPY(ahrs_aligner.lp_accel, imu.accel); VECT3_COPY(ahrs_aligner.lp_mag, imu.mag); RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro); // DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro); ahrs_align(); // DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias); #endif #ifdef DISABLE_ALIGNEMENT printf("# DISABLE_ALIGNEMENT\n"); #endif #ifdef DISABLE_PROPAGATE printf("# DISABLE_PROPAGATE\n"); #endif #ifdef DISABLE_ACCEL_UPDATE printf("# DISABLE_ACCEL_UPDATE\n"); #endif #ifdef DISABLE_MAG_UPDATE printf("# DISABLE_MAG_UPDATE\n"); #endif printf("# AHRS_TYPE %s\n", ahrs_type_str[AHRS_TYPE]); printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY); #ifdef AHRS_PROPAGATE_LOW_PASS_RATES printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n"); #endif #if AHRS_MAG_UPDATE_YAW_ONLY printf("# AHRS_MAG_UPDATE_YAW_ONLY\n"); #endif #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n"); #endif #if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n"); #endif #ifdef PERFECT_SENSORS printf("# PERFECT_SENSORS\n"); #endif #if AHRS_USE_GPS_HEADING printf("# AHRS_USE_GPS_HEADING\n"); #endif #if USE_AHRS_GPS_ACCELERATIONS printf("# USE_AHRS_GPS_ACCELERATIONS\n"); #endif printf("# tajectory : %s\n", aos.traj->name); };
application_t *application_init(struct config *cfg) { DEBUG("application_init()"); assert(cfg != 0); application_t *app = calloc(1, sizeof(struct _application)); assert(app != 0); // Initialize graphics if(!(app->graphics = graphics_init(cfg->app_window_id))) { ERROR("Cannot initialize graphics"); goto error; } // Create atlases if(!(app->atlas1 = graphics_atlas_create(cfg->graphics_font_file, cfg->graphics_font_size_1)) || !(app->atlas2 = graphics_atlas_create(cfg->graphics_font_file, cfg->graphics_font_size_2))) { ERROR("Cannot create atlas"); goto error; } // Create image if(!(app->image = graphics_image_create(app->graphics, cfg->video_width, cfg->video_height, cfg->video_format, ANCHOR_CENTER))) { ERROR("Cannot create image"); goto error; } // Create HUD if(!(app->hud = graphics_hud_create(app->graphics, app->atlas1, cfg->graphics_font_color_1, cfg->graphics_font_size_1, cfg->video_hfov, cfg->video_vfov))) { ERROR("Cannot create HUD"); goto error; } // Initialize GPS memcpy(&app->gps_config, &cfg->gps_conf, sizeof(struct gps_config)); app->gps_config.userdata = app; app->gps_config.create_label = create_label_handler; app->gps_config.delete_label = delete_label_handler; if(!(app->gps = gps_init(cfg->gps_device, &app->gps_config))) { ERROR("Cannot initialize GPS"); goto error; } // Initialize IMU memcpy(&app->imu_config, &cfg->imu_conf, sizeof(struct imu_config)); if(!(app->imu = imu_init(cfg->imu_device, &app->imu_config))) { ERROR("Cannot initialize IMU"); goto error; } // Open video if(!(app->video = video_open(cfg->video_device, cfg->video_width, cfg->video_height, cfg->video_format, cfg->video_interlace))) { ERROR("Cannot open video device"); goto error; } // Copy arguments app->video_width = cfg->video_width; app->video_height = cfg->video_height; app->window_width = cfg->window_width; app->window_height = cfg->window_height; app->video_hfov = cfg->video_hfov; app->video_vfov = cfg->video_vfov; app->visible_distance = cfg->app_landmark_vis_dist; memcpy(app->label_color, cfg->graphics_font_color_2, 4); return app; error: if(app->video) video_close(app->video); if(app->gps) gps_free(app->gps); if(app->imu) imu_free(app->imu); if(app->image) graphics_drawable_free(app->image); if(app->hud) graphics_hud_free(app->hud); if(app->atlas1) graphics_atlas_free(app->atlas1); if(app->atlas2) graphics_atlas_free(app->atlas2); if(app->graphics) graphics_free(app->graphics); free(app); return NULL; }
void gyro_offset_calibration() { uint8_t i; int16_t prev_gyro[3]; int16_t gyro[3]; float gyro_offset[3]; int8_t tilt_detected = 0; int16_t gyro_calibration_counter = GYRO_ITERATIONS; // TODO: Implement following function in spike_328p_i2c imu_set_dlpf(); // TODO: Possibly implement in a tick process // wait 2 seconds //_delay_ms(2000); //LOG("wait 2 sec...\r\n"); delay_millis(2000); while(gyro_calibration_counter > 0) { if(gyro_calibration_counter == GYRO_ITERATIONS) { // TODO: Possibly implement in a tick process delay_millis(700); // TODO: Implement following function in spike_328p_i2c imu_get_rotation(&gyro[0], &gyro[1], &gyro[2]); for(i=0; i<3; i++) { gyro_offset[i] = 0; prev_gyro[i] = gyro[i]; } } //LOG("imu_get_rotation:\r\n"); imu_get_rotation(&gyro[0], &gyro[1], &gyro[2]); for (i=0; i<3; i++) { if(abs(prev_gyro[i] - gyro[i]) > TOL) { tilt_detected++; #ifdef GYRO_DEBUG LOG("i=%d counter=%d diff=%d gyro[i]=%d, prev_gyro[i]=%d\r\n", i, gyro_calibration_counter, prev_gyro[i] - gyro[i], gyro[i], prev_gyro[i]); #endif break; } } for (i=0; i<3; i++) { gyro_offset[i] += (float)gyro[i]/GYRO_ITERATIONS; prev_gyro[i] = gyro[i]; } gyro_calibration_counter--; if(tilt_detected >= 1) { LOG("gyro calibration failed, retrying...\r\n"); gyro_calibration_counter = GYRO_ITERATIONS; tilt_detected = 0; } } // Put results into integer config.gyro_offset_x = (int16_t) gyro_offset[0]; config.gyro_offset_y = (int16_t) gyro_offset[1]; config.gyro_offset_z = (int16_t) gyro_offset[2]; LOG("updating gyro calibration offsets...\r\n"); LOG("config.gyro_offset[x,y,z]: %ld\t%ld\t%ld\r\n", config.gyro_offset_x, config.gyro_offset_y, config.gyro_offset_z); // TODO: Review imu_init(); }
int main(void) { clk_init(); gpio_init(); #ifdef SERIAL serial_init(); #endif i2c_init(); spi_init(); pwm_init(); pwm_set(MOTOR_FL, 0); // FL pwm_set(MOTOR_FR, 0); pwm_set(MOTOR_BL, 0); // BL pwm_set(MOTOR_BR, 0); // FR time_init(); if (RCC_GetCK_SYSSource() == 8) { } else { failloop(5); } sixaxis_init(); if (sixaxis_check()) { #ifdef SERIAL printf(" MPU found \n"); #endif } else { #ifdef SERIAL printf("ERROR: MPU NOT FOUND \n"); #endif failloop(4); } adc_init(); rx_init(); int count = 0; vbattfilt = 0.0; while (count < 64) { vbattfilt += adc_read(1); count++; } // for randomising MAC adddress of ble app - this will make the int = raw float value random_seed = *(int *)&vbattfilt ; random_seed = random_seed&0xff; vbattfilt = vbattfilt / 64; #ifdef SERIAL printf("Vbatt %2.2f \n", vbattfilt); #ifdef NOMOTORS printf("NO MOTORS\n"); #warning "NO MOTORS" #endif #endif #ifdef STOP_LOWBATTERY // infinite loop if (vbattfilt < STOP_LOWBATTERY_TRESH) failloop(2); #endif // loads acc calibration and gyro dafaults loadcal(); gyro_cal(); rgb_init(); imu_init(); extern unsigned int liberror; if (liberror) { #ifdef SERIAL printf("ERROR: I2C \n"); #endif failloop(7); } lastlooptime = gettime(); extern int rxmode; extern int failsafe; float thrfilt; // // // MAIN LOOP // // checkrx(); while (1) { // gettime() needs to be called at least once per second maintime = gettime(); looptime = ((uint32_t) (maintime - lastlooptime)); if (looptime <= 0) looptime = 1; looptime = looptime * 1e-6f; if (looptime > 0.02f) // max loop 20ms { failloop(3); //endless loop } lastlooptime = maintime; if (liberror > 20) { failloop(8); // endless loop } sixaxis_read(); control(); // battery low logic float hyst; float battadc = adc_read(1); vbatt = battadc; // average of all 4 motor thrusts // should be proportional with battery current extern float thrsum; // from control.c // filter motorpwm so it has the same delay as the filtered voltage // ( or they can use a single filter) lpf ( &thrfilt , thrsum , 0.9968f); // 0.5 sec at 1.6ms loop time lpf ( &vbattfilt , battadc , 0.9968f); #ifdef AUTO_VDROP_FACTOR static float lastout[12]; static float lastin[12]; static float vcomp[12]; static float score[12]; static int current_index = 0; int minindex = 0; float min = score[0]; { int i = current_index; vcomp[i] = vbattfilt + (float) i *0.1f * thrfilt; if ( lastin[i] < 0.1f ) lastin[i] = vcomp[i]; float temp; // y(n) = x(n) - x(n-1) + R * y(n-1) // out = in - lastin + coeff*lastout // hpf temp = vcomp[i] - lastin[i] + FILTERCALC( 1000*12 , 1000e3) *lastout[i]; lastin[i] = vcomp[i]; lastout[i] = temp; lpf ( &score[i] , fabsf(temp) , FILTERCALC( 1000*12 , 10e6 ) ); } current_index++; if ( current_index >= 12 ) current_index = 0; for ( int i = 0 ; i < 12; i++ ) { if ( score[i] < min ) { min = score[i]; minindex = i; } } #undef VDROP_FACTOR #define VDROP_FACTOR minindex * 0.1f #endif if ( lowbatt ) hyst = HYST; else hyst = 0.0f; vbatt_comp = vbattfilt + (float) VDROP_FACTOR * thrfilt; if ( vbatt_comp <(float) VBATTLOW + hyst ) lowbatt = 1; else lowbatt = 0; // led flash logic if (rxmode != RX_MODE_BIND) { // non bind if (failsafe) { if (lowbatt) ledflash(500000, 8); else ledflash(500000, 15); } else { if (lowbatt) ledflash(500000, 8); else { if (ledcommand) { if (!ledcommandtime) ledcommandtime = gettime(); if (gettime() - ledcommandtime > 500000) { ledcommand = 0; ledcommandtime = 0; } ledflash(100000, 8); } else { if ( aux[LEDS_ON] ) ledon( 255); else ledoff( 255); } } } } else { // bind mode ledflash(100000 + 500000 * (lowbatt), 12); } // rgb strip logic #if (RGB_LED_NUMBER > 0) extern void rgb_led_lvc( void); rgb_led_lvc( ); #endif #ifdef BUZZER_ENABLE buzzer(); #endif #ifdef FPV_ON static int fpv_init = 0; if ( rxmode == RX_MODE_NORMAL && ! fpv_init ) { fpv_init = gpio_init_fpv(); } if ( fpv_init ) { if ( failsafe ) { GPIO_WriteBit( FPV_PIN_PORT, FPV_PIN, Bit_RESET ); } else { GPIO_WriteBit( FPV_PIN_PORT, FPV_PIN, aux[ FPV_ON ] ? Bit_SET : Bit_RESET ); } } #endif checkrx(); // loop time 1ms while ((gettime() - maintime) < (1000 - 22) ) delay(10); } // end loop }
int main(void) { clk_init(); gpio_init(); #ifdef SERIAL serial_init(); #endif i2c_init(); spi_init(); pwm_init(); pwm_set(MOTOR_FL, 0); // FL pwm_set(MOTOR_FR, 0); pwm_set(MOTOR_BL, 0); // BL pwm_set(MOTOR_BR, 0); // FR time_init(); #ifdef SERIAL printf("\n clock source:"); #endif if (RCC_GetCK_SYSSource() == 8) { #ifdef SERIAL printf(" PLL \n"); #endif } else { #ifdef SERIAL if (RCC_GetCK_SYSSource() == 0) printf(" HSI \n"); else printf(" OTHER \n"); #endif failloop(5); } sixaxis_init(); if (sixaxis_check()) { #ifdef SERIAL printf(" MPU found \n"); #endif } else { #ifdef SERIAL printf("ERROR: MPU NOT FOUND \n"); #endif failloop(4); } adc_init(); rx_init(); int count = 0; float vbattfilt = 0.0; while (count < 64) { vbattfilt += adc_read(1); count++; } vbattfilt = vbattfilt / 64; #ifdef SERIAL printf("Vbatt %2.2f \n", vbattfilt); #ifdef NOMOTORS printf("NO MOTORS\n"); #warning "NO MOTORS" #endif #endif #ifdef STOP_LOWBATTERY // infinite loop if (vbattfilt < STOP_LOWBATTERY_TRESH) failloop(2); #endif // loads acc calibration and gyro dafaults loadcal(); gyro_cal(); imu_init(); extern unsigned int liberror; if (liberror) { #ifdef SERIAL printf("ERROR: I2C \n"); #endif failloop(7); } lastlooptime = gettime(); extern int rxmode; extern int failsafe; float thrfilt; // // // MAIN LOOP // // checkrx(); while (1) { // gettime() needs to be called at least once per second maintime = gettime(); looptime = ((uint32_t) (maintime - lastlooptime)); if (looptime <= 0) looptime = 1; looptime = looptime * 1e-6f; if (looptime > 0.02f) // max loop 20ms { failloop(3); //endless loop } lastlooptime = maintime; if (liberror > 20) { failloop(8); // endless loop } sixaxis_read(); control(); // battery low logic float battadc = adc_read(1); // average of all 4 motor thrusts // should be proportional with battery current extern float thrsum; // from control.c // filter motorpwm so it has the same delay as the filtered voltage // ( or they can use a single filter) lpf(&thrfilt, thrsum, 0.9968); // 0.5 sec at 1.6ms loop time lpf(&vbattfilt, battadc, 0.9968); if (vbattfilt + VDROP_FACTOR * thrfilt < VBATTLOW) lowbatt = 1; else lowbatt = 0; // led flash logic if (rxmode != RX_MODE_BIND) { // non bind if (failsafe) { if (lowbatt) ledflash(500000, 8); else ledflash(500000, 15); } else { if (lowbatt) ledflash(500000, 8); else { if (ledcommand) { if (!ledcommandtime) ledcommandtime = gettime(); if (gettime() - ledcommandtime > 500000) { ledcommand = 0; ledcommandtime = 0; } ledflash(100000, 8); } else ledon(255); } } } else { // bind mode ledflash(100000 + 500000 * (lowbatt), 12); } checkrx(); #ifdef DEBUG elapsedtime = gettime() - maintime; #endif // loop time 1ms while ((gettime() - maintime) < 1000) delay(10); } // end loop }
void imu_reset() { mpu.reset(); imu_init(); }
void init_ap( void ) { #ifndef SINGLE_MCU /** init done in main_fbw in single MCU */ mcu_init(); #endif /* SINGLE_MCU */ /************* Sensors initialization ***************/ #if USE_GPS gps_init(); #endif #ifdef USE_GPIO GpioInit(); #endif #if USE_IMU imu_init(); #endif #if USE_AHRS_ALIGNER ahrs_aligner_init(); #endif #if USE_AHRS ahrs_init(); #endif #if USE_INS ins_init(); #endif /************* Links initialization ***************/ #if defined MCU_SPI_LINK link_mcu_init(); #endif #if USE_AUDIO_TELEMETRY audio_telemetry_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 timers for periodic functions *****/ sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL); navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL); attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL); modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL); telemetry_tid = sys_time_register_timer(1./60, NULL); monitor_tid = sys_time_register_timer(1.0, NULL); /** - start interrupt task */ mcu_int_enable(); #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 }
// * imu_poll_gyro ************************************************************ // * poll gyro machine * // **************************************************************************** void imu_poll_gyro(void) { signed long calctemp; // calculation temporary variable float floattemp; // floating point temporary variable //retval = imu_i2c_start_transaction(IMU_ADDR_GYRO, 0x0F, imu_buff, 1, true, imu_catch_gyro); switch(gyro_machine_state) { case GYRO_MACHINE_START_I2C: // init I2C state { imu_init(); // initialize gyro interface // start transactions to be caught by next state txn_done = false; // transaction not done by default imu_buff[0] = 0x0F; // load configuration imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_CTRL_REG1, imu_buff, 1, false, imu_catch_gyro); // start sending first configuration gyro_machine_state = GYRO_MACHINE_CONFIG_1; // go to first configuration state } break; case GYRO_MACHINE_CONFIG_1: // configuration 1 - init reg 1 { //imu_i2c_start_transaction(unsigned char dev_address, unsigned char reg_address, unsigned char* data, unsigned int data_byte_count, tBoolean dir_is_receive, imu_i2c_xfer_done_t done_callback) if(txn_done) // previous config done, next config { txn_done = false; // transaction not done by default imu_buff[0] = 0x20; // load configuration (FULL SCALE 2000degrees/second) imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_CTRL_REG4, imu_buff, 1, false, imu_catch_gyro); // start sending first configuration gyro_machine_state = GYRO_MACHINE_CONFIG_2; // go to 2nd configuration state } } break; case GYRO_MACHINE_CONFIG_2: // configuration 2 - init reg 4 { if(txn_done) // previous config done, next ask for first sample from gyro to start averaging at rest, find zero point { txn_done = false; // transaction not done by default imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_OUT_Z, imu_buff, 2, true, imu_catch_gyro); // start asking for first read avg_count = IMU_GYRO_AVERAGES; // load down-counter with how many averages to take zero_point = 0; // clear any previous zero point gyro_machine_state = GYRO_MACHINE_ZEROING; // go to zeroing state } } break; case GYRO_MACHINE_ZEROING: // stationary condition averaging { if(txn_done) // previous config done, next ask for first sample from gyro to start averaging at rest, find zero point { txn_done = false; // transaction not done by default zero_point += ((signed long)(*(signed short*)imu_buff)); // accumulate previous rest value imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_OUT_Z, imu_buff, 2, true, imu_catch_gyro); // ask for next rest value avg_count--; // another sample read if(avg_count == 0) { zero_point /= ((signed long)IMU_GYRO_AVERAGES); // finish average calculation, now have zero point at rest, you can now move the gyro imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_OUT_Z, imu_buff, 2, true, imu_catch_gyro); // ask for next (possibly dynamic) value to begin integrating heading_millidegrees = 0; // initialize heading to zero gyro_machine_state = GYRO_MACHINE_RUNNING; // go to running (integrating) state now that we have the zero point } } } break; case GYRO_MACHINE_RUNNING: // normal operation - integrating deltas { if(txn_done) // previous config done, next ask for first sample from gyro to start averaging at rest, find zero point { txn_done = false; // transaction not done by default calctemp = ((signed long)(*(signed short*)imu_buff)); // grab 2B 2's complement sample imu_i2c_start_transaction(IMU_ADDR_GYRO, IMU_GYRO_OUT_Z, imu_buff, 2, true, imu_catch_gyro); // ask for next rest value floattemp = (float)(calctemp-zero_point); // would be better to leave this value raw then convert later to avoid accumulating error but this is faster to implement for now, note subtract out DC offset yaw_rate_millidps = (signed long)(floattemp * ((float)IMU_GYRO_YAW_RATE_CONVERT_FACTOR)); // update yaw rate floattemp *= (float)IMU_GYRO_CONVERT_FACTOR; // multiply delta by conversion factor heading_millidegrees += (signed long)floattemp; // accumulate/integrate sample } } break; default: gyro_machine_state = GYRO_MACHINE_START_I2C; // get machine back on track, should never be here break; } }