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
