int mot_deinit(uquad_mot_t *mot) { int retval; retval = system(KILL_MOTOR_CMD); if(mot != NULL) { uquad_kmsgq_deinit(mot->kmsgq); if(mot->tx_log != NULL) fclose(mot->tx_log); uquad_mat_free(mot->w_curr); free(mot); }; return retval;; };
/** * Interrumpe ejecucion del programa. Dependiendo del valor del parametro * que se le pase interrumpe mas o menos cosas. * Q == 4 : interrupcion por muerte de gpsd, cierra todo menos gpsd * Q == 3 : interrupcion por muerte del parser, cierra todo menos parser * Q == 2 : interrupcion manual (ctrl-c), cierra todo y avisa que fue manual * Q == 1 : interrupcion por muerte del sbusd, cierra todo menos sbusd * Q == 0 : interrupcion estandar, cierra todo * Q cualquier otro : igual que Q == 0. * * TODO QUE HACER CUANDO ALGO FALLA Y NECESITAMOS APAGAR LOS MOTORES! * TODO SI SE CIERRA GPSD TRATAR DE ABRIRLO DENUEVO? */ void quit(int Q) { int retval; if(Q == 2) { err_log("interrumpido manualmente (ctrl-c)"); //TODO no funco bien en la beagle } else err_log("algo salio mal, cerrando..."); if(Q != 1) { /// Terminar Demonio S-BUS retval = system(KILL_SBUS); //sprintf(str, "kill -SIGTERM %d", sbusd_child_pid); //retval = system(str); if (retval < 0) err_log("Could not terminate sbusd!"); } /* /// cerrar IO manager retval = io_deinit(io); if(retval != ERROR_OK) err_log("Could not close IO correctly!");*/ /// Kernel Messeges Queue uquad_kmsgq_deinit(kmsgq); /// Log close(log_fd); #if !SIMULATE_GPS if(Q != 4) { /// cerrar conexiones con GPSD y terminarlo retval = deinit_gps(); if(retval != ERROR_OK) err_log("Could not close gps correctly!"); } #endif // !SIMULATE_GPS #if !DISABLE_UAVTALK if(Q != 3) { /// cerrar UAVTalk retval = kill(uavtalk_child_pid, SIGKILL); if(retval != ERROR_OK) err_log("Could not close Parser correctly!"); } #endif // !DISABLE_UAVTALK exit(0); }