void FCF_Init (libusbSource * src) { init_gps(src); init_theo_imu(src); init_adis_imu(src); init_mouse(src); //read from usb mouse; set your hw values in is_mouse() init_mouse2(src); //read from a 2nd usb mouse; set your hw values in is_mouse2() //InitProfiling(); //init_virtgyro(src); //read from socket }
int main(int argc, char **argv) { static struct Gps gps; init_gps(&gps, argv[0]); //init threads g_thread_init(NULL); gdk_threads_init(); //init gtk gtk_init(&argc, &argv); create_window(&gps); gdk_threads_enter(); gtk_main(); //루프 처리 gdk_threads_leave(); return 0; }
// It all starts here: int main(void) { // start with default user settings in case there's nothing in eeprom default_user_settings(); // try to load settings from eeprom read_user_settings_from_eeprom(); // set our LED as a digital output lib_digitalio_initpin(LED1_OUTPUT,DIGITALOUTPUT); //initialize the libraries that require initialization lib_timers_init(); lib_i2c_init(); // pause a moment before initializing everything. To make sure everything is powered up lib_timers_delaymilliseconds(100); // initialize all other modules init_rx(); init_outputs(); serial_init(); init_gyro(); init_acc(); init_baro(); init_compass(); init_gps(); init_imu(); // set the default i2c speed to 400 KHz. If a device needs to slow it down, it can, but it should set it back. lib_i2c_setclockspeed(I2C_400_KHZ); // initialize state global.state.armed=0; global.state.calibratingCompass=0; global.state.calibratingAccAndGyro=0; global.state.navigationMode=NAVIGATION_MODE_OFF; global.failsafeTimer=lib_timers_starttimer(); // run loop for(;;) { // check to see what switches are activated check_checkbox_items(); // check for config program activity serial_check_for_action(); calculate_timesliver(); // run the imu to estimate the current attitude of the aircraft imu_calculate_estimated_attitude(); // arm and disarm via rx aux switches if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) { // see if we want to change armed modes if (!global.state.armed) { if (global.activeCheckboxItems & CHECKBOX_MASK_ARM) { global.state.armed=1; #if (GPS_TYPE!=NO_GPS) navigation_set_home_to_current_location(); #endif global.home.heading=global.currentEstimatedEulerAttitude[YAW_INDEX]; global.home.location.altitude=global.baroRawAltitude; } } else if (!(global.activeCheckboxItems & CHECKBOX_MASK_ARM)) global.state.armed=0; } #if (GPS_TYPE!=NO_GPS) // turn on or off navigation when appropriate if (global.state.navigationMode==NAVIGATION_MODE_OFF) { if (global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME) { // return to home switch turned on navigation_set_destination(global.home.location.latitude,global.home.location.longitude); global.state.navigationMode=NAVIGATION_MODE_RETURN_TO_HOME; } else if (global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD) { // position hold turned on navigation_set_destination(global.gps.currentLatitude,global.gps.currentLongitude); global.state.navigationMode=NAVIGATION_MODE_POSITION_HOLD; } } else { // we are currently navigating // turn off navigation if desired if ((global.state.navigationMode==NAVIGATION_MODE_RETURN_TO_HOME && !(global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME)) || (global.state.navigationMode==NAVIGATION_MODE_POSITION_HOLD && !(global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD))) { global.state.navigationMode=NAVIGATION_MODE_OFF; // we will be turning control back over to the pilot. reset_pilot_control(); } } #endif // read the receiver read_rx(); // turn on the LED when we are stable and the gps has 5 satelites or more #if (GPS_TYPE==NO_GPS) lib_digitalio_setoutput(LED1_OUTPUT, (global.state.stable==0)==LED1_ON); #else lib_digitalio_setoutput(LED1_OUTPUT, (!(global.state.stable && global.gps.numSatelites>=5))==LED1_ON); #endif // get the angle error. Angle error is the difference between our current attitude and our desired attitude. // It can be set by navigation, or by the pilot, etc. fixedpointnum angleError[3]; // let the pilot control the aircraft. get_angle_error_from_pilot_input(angleError); #if (GPS_TYPE!=NO_GPS) // read the gps unsigned char gotNewGpsReading=read_gps(); // if we are navigating, use navigation to determine our desired attitude (tilt angles) if (global.state.navigationMode!=NAVIGATION_MODE_OFF) { // we are navigating navigation_set_angle_error(gotNewGpsReading,angleError); } #endif if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) { // We are probably on the ground. Don't accumnulate error when we can't correct it reset_pilot_control(); // bleed off integrated error by averaging in a value of zero lib_fp_lowpassfilter(&global.integratedAngleError[ROLL_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0); lib_fp_lowpassfilter(&global.integratedAngleError[PITCH_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0); lib_fp_lowpassfilter(&global.integratedAngleError[YAW_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0); } #ifndef NO_AUTOTUNE // let autotune adjust the angle error if the pilot has autotune turned on if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOTUNE) { if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE)) { autotune(angleError,AUTOTUNE_STARTING); // tell autotune that we just started autotuning } else { autotune(angleError,AUTOTUNE_TUNING); // tell autotune that we are in the middle of autotuning } } else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE) { autotune(angleError,AUTOTUNE_STOPPING); // tell autotune that we just stopped autotuning } #endif // This gets reset every loop cycle // keep a flag to indicate whether we shoud apply altitude hold. The pilot can turn it on or // uncrashability/autopilot mode can turn it on. global.state.altitudeHold=0; if (global.activeCheckboxItems & CHECKBOX_MASK_ALTHOLD) { global.state.altitudeHold=1; if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_ALTHOLD)) { // we just turned on alt hold. Remember our current alt. as our target global.altitudeHoldDesiredAltitude=global.altitude; global.integratedAltitudeError=0; } } fixedpointnum throttleOutput; #ifndef NO_AUTOPILOT // autopilot is available if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOPILOT) { if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT)) { // let autopilot know to transition to the starting state autopilot(AUTOPILOT_STARTING); } else { // autopilot normal run state autopilot(AUTOPILOT_RUNNING); } } else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT) { // tell autopilot that we just stopped autotuning autopilot(AUTOPILOT_STOPPING); } else { // get the pilot's throttle component // convert from fixedpoint -1 to 1 to fixedpoint 0 to 1 throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET; } #else // get the pilot's throttle component // convert from fixedpoint -1 to 1 to fixedpoint 0 to 1 throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET; #endif #ifndef NO_UNCRASHABLE uncrashable(gotNewGpsReading,angleError,&throttleOutput); #endif #if (BAROMETER_TYPE!=NO_BAROMETER) // check for altitude hold and adjust the throttle output accordingly if (global.state.altitudeHold) { global.integratedAltitudeError+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,global.timesliver); lib_fp_constrain(&global.integratedAltitudeError,-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT); // don't let the integrated error get too high // do pid for the altitude hold and add it to the throttle output throttleOutput+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,settings.pid_pgain[ALTITUDE_INDEX])-lib_fp_multiply(global.altitudeVelocity,settings.pid_dgain[ALTITUDE_INDEX])+lib_fp_multiply(global.integratedAltitudeError,settings.pid_igain[ALTITUDE_INDEX]); } #endif #ifndef NO_AUTOTHROTTLE if ((global.activeCheckboxItems & CHECKBOX_MASK_AUTOTHROTTLE) || global.state.altitudeHold) { if (global.estimatedDownVector[Z_INDEX]>FIXEDPOINTCONSTANT(.3)) { // Divide the throttle by the throttleOutput by the z component of the down vector // This is probaly the slow way, but it's a way to do fixed point division fixedpointnum recriprocal=lib_fp_invsqrt(global.estimatedDownVector[Z_INDEX]); recriprocal=lib_fp_multiply(recriprocal,recriprocal); throttleOutput=lib_fp_multiply(throttleOutput-AUTOTHROTTLE_DEAD_AREA,recriprocal)+AUTOTHROTTLE_DEAD_AREA; } } #endif #ifndef NO_FAILSAFE // if we don't hear from the receiver for over a second, try to land safely if (lib_timers_gettimermicroseconds(global.failsafeTimer)>1000000L) { throttleOutput=FPFAILSAFEMOTOROUTPUT; // make sure we are level! angleError[ROLL_INDEX]=-global.currentEstimatedEulerAttitude[ROLL_INDEX]; angleError[PITCH_INDEX]=-global.currentEstimatedEulerAttitude[PITCH_INDEX]; } #endif // calculate output values. Output values will range from 0 to 1.0 // calculate pid outputs based on our angleErrors as inputs fixedpointnum pidOutput[3]; // Gain Scheduling essentialy modifies the gains depending on // throttle level. If GAIN_SCHEDULING_FACTOR is 1.0, it multiplies PID outputs by 1.5 when at full throttle, // 1.0 when at mid throttle, and .5 when at zero throttle. This helps // eliminate the wobbles when decending at low throttle. fixedpointnum gainSchedulingMultiplier=lib_fp_multiply(throttleOutput-FIXEDPOINTCONSTANT(.5),FIXEDPOINTCONSTANT(GAIN_SCHEDULING_FACTOR))+FIXEDPOINTONE; for (int x=0;x<3;++x) { global.integratedAngleError[x]+=lib_fp_multiply(angleError[x],global.timesliver); // don't let the integrated error get too high (windup) lib_fp_constrain(&global.integratedAngleError[x],-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT); // do the attitude pid pidOutput[x]=lib_fp_multiply(angleError[x],settings.pid_pgain[x])-lib_fp_multiply(global.gyrorate[x],settings.pid_dgain[x])+(lib_fp_multiply(global.integratedAngleError[x],settings.pid_igain[x])>>4); // add gain scheduling. pidOutput[x]=lib_fp_multiply(gainSchedulingMultiplier,pidOutput[x]); } lib_fp_constrain(&throttleOutput,0,FIXEDPOINTONE); // Keep throttle output between 0 and 1 compute_mix(throttleOutput, pidOutput); // aircraft type dependent mixes #if (NUM_SERVOS>0) // do not update servos during unarmed calibration of sensors which are sensitive to vibration if (global.state.armed || (!global.state.calibratingAccAndGyro)) write_servo_outputs(); #endif write_motor_outputs(); }
int main(void) { Config32MHzClock(); // Setup the 32MHz Clock. Should really be using 2MHz... // Setup output and input ports. LEDPORT.DIRSET = 0xFF; LEDPORT.OUT = 0xFF; AD9835_PORT.DIRCLR = 0x40; PORTC.DIRSET = 0x04; // Start up the timer. init_timer(); sensors.begin(); sensors.requestTemperatures(); // Wait a bit before starting the AD9835. // It seems to take a few hundred ms to 'boot up' once power is applied. _delay_ms(500); // Configure the AD9835, and start in sleep mode. AD9835_Setup(); AD9835_Sleep(); // Setup the AD9835 for our chosen datamode. TX_Setup(); AD9835_Awake(); // Broadcast a bit of carrier. _delay_ms(1000); TXString("Booting up...\n"); // Kind of like debug lines. // Start up the GPS RX UART. init_gps(); // Turn Interrupts on. PMIC.CTRL = PMIC_HILVLEN_bm | PMIC_LOLVLEN_bm; sei(); sendNMEA("$PUBX,00"); // Poll the UBlox5 Chip for data. //TXString("GPS Active, Interrupts On.\n"); int found_sensors = sensors.getDeviceCount(); //sprintf(tx_buffer,"Found %u sensors.\n",found_sensors); // TXString(tx_buffer); unsigned int counter = 0; // Init out TX counter. while(1){ // Identify every few minutes if ((counter%30 == 0)&&(data_mode != FALLBACK)) TXString("DE VK5VZI Project Horus HAB Launch - projecthorus.org \n"); // Read ADC PortA pin 0, using differential, signed input mode. Negative input comes from pin 1, which is tied to ground. Use VCC/1.6 as ref. uint16_t temp = readADC(); float bat_voltage = (float)temp * 0.001007572056668* 8.5; floatToString(bat_voltage,1,voltString); // Collect GPS data gps.f_get_position(&lat, &lon); sats = gps.sats(); if(sats>2){LEDPORT.OUTCLR = 0x80;} speed = gps.f_speed_kmph(); altitude = (long)gps.f_altitude(); gps.crack_datetime(0, 0, 0, &time[0], &time[1], &time[2]); floatToString(lat, 5, latString); floatToString(lon, 5, longString); sensors.requestTemperatures(); _intTemp = sensors.getTempC(internal); _extTemp = sensors.getTempC(external); if (_intTemp!=85 && _intTemp!=127 && _intTemp!=-127 && _intTemp!=999) intTemp = _intTemp; if (_extTemp!=85 && _extTemp!=127 && _extTemp!=-127 && _extTemp!=999) extTemp = _extTemp; if(data_mode != FALLBACK){ // Construct our Data String sprintf(tx_buffer,"$$DARKSIDE,%u,%02d:%02d:%02d,%s,%s,%ld,%d,%d,%d,%d,%s",counter++,time[0], time[1], time[2],latString,longString,altitude,speed,sats,intTemp,extTemp,voltString); // Calculate the CRC-16 Checksum char checksum[10]; snprintf(checksum, sizeof(checksum), "*%04X\n", gps_CRC16_checksum(tx_buffer)); // And copy the checksum onto the end of the string. memcpy(tx_buffer + strlen(tx_buffer), checksum, strlen(checksum) + 1); }else{ // If our battery is really low, we don't want to transmit much data, so limit what we TX to just an identifier, battery voltage, and our position. sprintf(tx_buffer, "DE VK5VZI HORUS8 %s %s %s %ld", bat_voltage, latString, longString,altitude); } // Blinky blinky... LEDPORT.OUTTGL = 0x20; // Transmit! TXString(tx_buffer); sendNMEA("$PUBX,00"); // Poll the UBlox5 Chip for data again. /* // Check the battery voltage. If low, switch to a more reliable mode. if((bat_voltage < BATT_THRESHOLD) && (data_mode != RELIABLE_MODE)){ new_mode = RELIABLE_MODE; // This string should be changed if the 'reliable' mode is changed. TXString("Battery Voltage Below 9V. Switching to DominoEX8.\n"); } */ // Perform a mode switch, if required. // Done here to allow for mode changes to occur elsewhere. if(new_mode != -1){ data_mode = new_mode; TX_Setup(); new_mode = -1; } // And wait a little while before sending the next string. // Don't delay for domino - synch stuffs up otherwise if(data_mode != DOMINOEX8){ _delay_ms(1000); } } }
int main(int argc, char *argv[]) { #if !DISABLE_IMU #if SIMULATE_ALTITUDE #error No se puede simular altura con la IMU activada! #endif #endif /// mensajes al usuario #if PC_TEST printf("----------------------\n WARN: Modo 'PC test' - Ver common/quadcop_config.h \n----------------------\n"); sleep_ms(500); #endif #if SIMULATE_GPS printf("----------------------\n WARN: GPS simulado - Ver common/quadcop_config.h \n----------------------\n"); sleep_ms(500); #endif #if DISABLE_UAVTALK printf("----------------------\n WARN: UAVTalk desabilitado - Ver common/quadcop_config.h \n----------------------\n"); sleep_ms(500); #endif #if DISABLE_IMU printf("----------------------\n WARN: IMU desabilitada - Ver common/quadcop_config.h \n----------------------\n"); sleep_ms(500); #endif /// Init socket comm #if SOCKET_TEST socket_comm_init(); #endif int retval; int err_imu = 0; //Aumenta si no hay datos nuevos de imu int err_count_no_data = 0; //si no tengo datos nuevos varias veces es peligroso int no_gps_data = 0; // Para log char* log_name; char buff_log[512]; //TODO determinar valor char buff_log_aux[512]; //TODO determinar valor int buff_log_len; bool first_time = true; // para saber cuando es la primer ejecucion del loop int8_t count_50 = 1; // controla tiempo de loop 100ms if(argc<3) { err_log("USAGE: ./auto_pilot log_name thrust_hovering"); exit(1); } else { log_name = argv[1]; thrust_hovering = atoi(argv[2]); if( thrust_hovering < 16 || thrust_hovering > 40) { puts("Con este throttle dudo que hagas hovering, cerrando"); //exit(0); } printf("Throttle hovering: %u\n", (uint16_t) (thrust_hovering*17.41+1212.53) ); } // Configurar pin de uart1 tx #if !PC_TEST retval = system("echo 2 > /sys/kernel/debug/omap_mux/dss_data6"); if (retval < 0) { err_log("system() failed - UART1 mux config"); exit(0); } #endif //setea senales y mascara set_signals(); // Control de tiempos struct timeval tv_in_loop, tv_out_loop, tv_out_last_loop, tv_start_main, tv_diff; #if DEBUG struct timeval dt; #endif // -- -- -- -- -- -- -- -- -- // Inicializacion // -- -- -- -- -- -- -- -- -- /// Tiempo set_main_start_time(); tv_start_main = get_main_start_time(); //uquad_aux_time.h #if SOCKET_TEST if (socket_comm_wait_client() == -1) exit(0); #endif /// Path planning & Following // init lista path lista_path = (Lista_path *)malloc(sizeof(struct ListaIdentificar_path)); inicializacion_path(lista_path); // init lista wp lista_way_point = (Lista_wp *)malloc(sizeof(struct ListaIdentificar_wp)); inicializacion_wp(lista_way_point); retval = way_points_input(lista_way_point); //carga waypoints en la lista desde un archivo de texto if (retval < 0) { puts("No se pudo cargar lista de waypoints, cerrando"); exit(0); } // Generacion de trayectoria path_planning(lista_way_point, lista_path); log_trayectoria(lista_path); //dbg //visualizacion_path(lista_path); // dbg #if SOCKET_TEST if (socket_comm_send_path(lista_path) == -1) exit(0); #endif /// Control yaw #if CONTROL_YAW_ADD_DERIVATIVE control_yaw_init_error_buff(); #endif /// Control velocidad // TODO // Se calcula el valor de pitch necesario para alcanzar la velocidad deseada. // El signo negativo es para que sea coherente con el sentido de giro de la cc3d (angulo positivo = giro horario) pitch = -atan(B_ROZ*VEL_DESIRED/MASA/G); //printf("pitch: %lf\n", pitch*180/M_PI); //dbg /// Control altura control_alt_init_error_buff(); //thrust_hovering = throttle_hovering*0.0694-88.81; printf("Thrust hovering: %lf\n", thrust_hovering); /// Log log_fd = open(log_name, O_RDWR | O_CREAT | O_NONBLOCK ); if(log_fd < 0) { err_log_stderr("Failed to open log file!"); exit(0); } bool log_writeOK; ///GPS config - Envia comandos al gps a traves del puerto serie - // #if !SIMULATE_GPS retval = preconfigure_gps(); if(retval < 0) { err_log("Failed to preconfigure gps!"); exit(0); } /// Ejecuta GPS daemon - proceso independiente gpsd_child_pid = init_gps(); if(gpsd_child_pid == -1) { err_log_stderr("Failed to init gps!"); exit(0); } #endif //!SIMULATE_GPS /// Ejecuta Demonio S-BUS - proceso independiente sbusd_child_pid = futaba_sbus_start_daemon(); if(sbusd_child_pid == -1) { err_log_stderr("Failed to start child process (sbusd)!"); quit(1); } /// inicializa kernel messages queues - para comunicacion con sbusd kmsgq = uquad_kmsgq_init(SERVER_KEY, DRIVER_KEY); if(kmsgq == NULL) { quit_log_if(ERROR_FAIL,"Failed to start message queue!"); } //Doy tiempo a que inicien bien los procesos secundarios sleep_ms(500); //TODO verificar cuanto es necesario /// inicializa UAVTalk #if !DISABLE_UAVTALK uavtalk_child_pid = uavtalk_parser_start(tv_start_main); if(uavtalk_child_pid == -1) { err_log_stderr("Failed to start child process (uavtalk)!"); quit(3); } #endif /// init IMU #if !DISABLE_IMU fd_IMU = imu_comm_init(IMU_DEVICE); if(fd_IMU < 0) { err_log("Failed to init IMU!"); quit(0); } //imu_data_alloc(&imu_data); //magn_calib_init(); int bytes_avail = 0; // Para obener bytes disponibles en el RX buffer de IMU #endif #if !DISABLE_IMU // Clean IMU serial buffer err_log("Clearing IMU input buffer..."); serial_flush(fd_IMU); sleep_ms(40); #endif #if !DISABLE_UAVTALK retval = uavtalk_init_shm(); if (retval < 0) { puts("Error al inicializar shm, cerrando"); quit(0); } #endif //!DISABLE_UAVTALK printf("----------------------\n Entrando al loop \n----------------------\n"); // -- -- -- -- -- -- -- -- -- // Loop // -- -- -- -- -- -- -- -- -- for(;;) { //para tener tiempo de entrada en cada loop gettimeofday(&tv_in_loop,NULL); //TODO Mejorar control de errores if(err_count_no_data > 10) { err_log("mas de 10 errores en recepcion de datos!"); // TODO que hago? me quedo en hovering hasta obtener datos? err_count_no_data = 0; //por ahora... } #if !DISABLE_IMU /// Lectura de datos de la IMU //-------------------------------------------------------------------------------------------------------- leer_imu: // Vuelvo a leer si es necesario if(err_imu > 2) { err_log("No hay datos nuevos de IMU, cerrando."); quit(0); } // Reviso si tengo una trama de datos completa antes de leer ioctl(fd_IMU, FIONREAD, &bytes_avail); if(bytes_avail < 34) { //puts("datos no completos IMU!"); //dbg sleep_ms(5); err_imu++; goto leer_imu; } IMU_readOK = check_read_locks(fd_IMU); if (IMU_readOK) { // Leo datos retval = imu_comm_read(fd_IMU); if (retval < 0 ) { puts("WARN: unable to read IMU data!"); } // Paso los datos del buffer RX a imu_raw. imu_comm_parse_frame_binary(&imu_raw); //print_imu_raw(&imu_raw); // dbg // Si no estoy calibrando convierto datos para usarlos if (baro_calibrated) { imu_raw2data(&imu_raw, &imu_data); //print_imu_data(&imu_data); //dbg } imu_updated = true; IMU_readOK = false; err_imu = 0; } else { err_log("IMU: read NOT ok"); } /* /// Reviso si quedan datos para no atrasarme IMU_readOK = check_read_locks(fd_IMU); if (IMU_readOK) { printf("todavia quedan datos IMU!\n"); IMU_readOK = false; ioctl(fd_IMU, FIONREAD, &bytes_avail); //printf("%d\n ",bytes_avail); if(bytes_avail > 33) goto leer_imu; //continue; }*/ // Reviso si tengo una trama de datos completa atrasada retval = ioctl(fd_IMU, FIONREAD, &bytes_avail); if (retval == ERROR_OK) { if(bytes_avail > 33) { //puts("todavia quedan datos IMU!"); //dbg goto leer_imu; } } else puts("FIONREAD failed!"); if (!baro_calibrated) { if(imu_updated) { po += imu_raw.pres; baro_calib_cont++; if (baro_calib_cont == BARO_CALIB_SAMPLES) { pres_calib_init(po/BARO_CALIB_SAMPLES); baro_calibrated = true; puts("Barometro calibrado!"); } } goto end_loop; //si estoy calibrando no hago nada mas! } //-------------------------------------------------------------------------------------------------------- #endif /** loop 50 ms **/ #if !DISABLE_UAVTALK /// Leo datos de CC3D retval = uavtalk_read(&act); if(retval < 0) { err_log("No hay datos nuevos de actitud, cerrando."); quit(0); } uavtalk_updated = true; //uav_talk_print_attitude(act); //dbg #endif // Calcula diferencia respecto a cero act.yaw = act.yaw - get_yaw_zero(); ++count_50; // control de loop 100ms /// Check stdin stdin_readOK = check_read_locks(STDIN_FILENO); if (stdin_readOK) { read_from_stdin(); } /** loop 100 ms **/ if(count_50 > 1) { #if !SIMULATE_GPS /// Obener datos del GPS retval = get_gps_data(&gps); if (retval < 0 ) { //que hago si NO hay datos!? err_log("No hay datos de gps"); } else { //que hago si SI hay datos!? printf("%lf\t%lf\t%lf\t%lf\t%lf\n", \ gps.latitude,gps.longitude,gps.altitude,gps.speed,gps.track); gps_updated = true; } #else //GPS SIMULADO if(control_status == STARTED && !first_time) gps_simulate_position(&position, &velocity, act.yaw, pitch); gps_updated = true; #endif //!SIMULATE_GPS count_50 = 0; } /** end loop 100 ms **/ if(control_status == STARTED) { if (first_time) first_time = false; //#if 0 /// Con datos de actitud hago control de yaw y path following (si tengo gps) if(uavtalk_updated) { /// Path Follower - si hay datos de gps y de yaw hago carrot chase if(gps_updated) { #if !SIMULATE_GPS /* guardo en waypoint p los valores de (x,y) hallados mediante el gps. * primero deberia hacer convert_gps2utm(&utm, gps) y restarle la utm inicial */ //convert_gps2waypoint(&wp, gps); // TODO no implementado!! wp.angulo = act.yaw; #else wp.x = position.x; wp.y = position.y; wp.z = position.z; wp.angulo = act.yaw; #endif //!SIMULATE_GPS //carrot chase retval = path_following(wp, lista_path, &yaw_d); if (retval == -1) { control_status = FINISHED; puts("¡¡ Trayectoria finalizada !!"); ch_buff[THROTTLE_CH_INDEX] = THROTTLE_NEUTRAL; // detengo los motores } no_gps_data = 0; } else { no_gps_data++; if (no_gps_data > 1) { // Debo tener datos del gps cada 2 loops de 50 ms err_count_no_data++; no_gps_data = 0; err_log("No tengo datos de gps para seguimiento de trayectorias"); } } /// Control Yaw - necesito solo medida de yaw u_yaw = control_yaw_calc_input(yaw_d, act.yaw); //printf("senal de control: %lf\n", u); // dbg //Convertir velocidad en comando ch_buff[YAW_CH_INDEX] = (uint16_t) (u_yaw*25/11 + 1500); //printf(" %u\n", ch_buff[YAW_CH_INDEX]); // dbg if (err_count_no_data > 0) err_count_no_data--; } else { err_log("No tengo datos para control de yaw"); err_count_no_data++; } /// Control de Velocidad if(gps_updated) { //TODO... } //#endif //if 0 /// Despegue if (takeoff == 1) { retval = control_altitude_takeoff(&h_d); if (retval > 0) takeoff = 0; } /// Aterrizaje if (takeoff == -1) { retval = control_altitude_land(&h_d); if (retval > 0) { takeoff = 0; //ch_buff[THROTTLE_CH_INDEX] = THROTTLE_NEUTRAL; puts("Pronto para aterrizar!"); //control_status = STOPPED; } } #if SIMULATE_ALTITUDE /// Control de Altura u_h = control_alt_calc_input(h_d, h); //U_h = u_h + 18.1485; U_h = u_h + thrust_hovering + control_alt_integral(h_d, h); //sim imu_simulate_altitude(&h, U_h, 0, 0); //Convertir empuje en comando if (U_h <= 0) { ch_buff[THROTTLE_CH_INDEX] = THROTTLE_NEUTRAL; } else if (U_h > 43.6) { ch_buff[THROTTLE_CH_INDEX] = MAX_COMMAND; } else { ch_buff[THROTTLE_CH_INDEX] = (uint16_t) (-0.2984*pow(U_h,2) + 26.0289*U_h + 1168.8); } //printf("h: %lf\th_d: %lf\tu_h: %lf\tU_h: %lf\tThrot: %u\n",h,h_d,u_h,U_h,ch_buff[THROTTLE_CH_INDEX]); // dbg #endif #if !DISABLE_IMU /// Control de Altura u_h = control_alt_calc_input(h_d, imu_data.us_altitude); //U_h = u_h + 18.1485; U_h = u_h + thrust_hovering + control_alt_integral(h_d, imu_data.us_altitude); //Convertir empuje en comando if (U_h <= 0) { ch_buff[THROTTLE_CH_INDEX] = THROTTLE_NEUTRAL; } else if (U_h > 22) { ch_buff[THROTTLE_CH_INDEX] = (uint16_t) (17.41*U_h+1212.53); } else { ch_buff[THROTTLE_CH_INDEX] = (uint16_t) (-0.2984*pow(U_h,2) + 26.0289*U_h + 1168.8); } #endif // Luego de finalizados los controles reseteo flags uavtalk_updated = false; gps_updated = false; imu_updated = false; } // end if(control_started) // Envia actitud y throttle deseados a sbusd (a traves de mensajes de kernel) retval = uquad_kmsgq_send(kmsgq, buff_out, MSGSZ); if(retval != ERROR_OK) { quit_log_if(ERROR_FAIL,"Failed to send message!"); } #if SOCKET_TEST if (socket_comm_update_position(position) == -1) quit(1); #endif /** Log - T_s_act/T_us_act/roll/pitch/yaw/C_roll/C_pitch/C_yaw/C_throt/T_s_main/T_us_main/pos.x/pos.y/pos.z/yaw_d **/ //Timestamp main uquad_timeval_substract(&tv_diff, tv_in_loop, tv_start_main); // Duracion del main gettimeofday(&tv_out_loop,NULL); uquad_timeval_substract(&dt, tv_out_loop, tv_out_last_loop); //datos de CC3D para log buff_log_len = uavtalk_to_str(buff_log, act); //otros logs buff_log_len += sprintf(buff_log_aux, "%u %u %u %u %lu %lu %lf %lf %lf %lf %lf %lf %lf", ch_buff[ROLL_CH_INDEX], ch_buff[PITCH_CH_INDEX], ch_buff[YAW_CH_INDEX], ch_buff[THROTTLE_CH_INDEX], tv_diff.tv_sec, tv_diff.tv_usec, position.x, position.y, position.z, yaw_d, u_yaw, h_d, U_h); strcat(buff_log, buff_log_aux); //datos de IMU para log buff_log_len += imu_to_str(buff_log_aux, imu_data); strcat(buff_log, buff_log_aux); log_writeOK = check_write_locks(log_fd); if (log_writeOK) { retval = write(log_fd, buff_log, buff_log_len); if(retval < 0) err_log_stderr("Failed to write to log file!"); } end_loop: /*#if DEBUG // checkeo de tiempos del main gettimeofday(&tv_out_loop,NULL); retval = uquad_timeval_substract(&dt, tv_out_loop, tv_in_loop); if(retval > 0) { if(dt.tv_usec > 50000) { err_log("WARN: se atraso main"); } } else { err_log("WARN: Main absurd timing!"); } #endif // DEBUG*/ /// Control de tiempos del loop wait_loop_T_US(MAIN_LOOP_50_MS,tv_in_loop); } // for(;;) return 0; //nunca llego aca } //FIN MAIN