void quit()
{
int ret;
#if !PC_TEST
ret = close(fd);
if(ret < 0)
{
err_log_stderr("Failed to close serial port!");
}
#else
ret = fclose(fp);
if(ret != 0)
{
err_log_stderr("Failed to close log file!");
}
#endif
fflush(stderr);
exit(1);
}
int check_io_locks(int fd, FILE *device, uquad_bool_t *read_ok, uquad_bool_t *write_ok){
fd_set rfds,wfds;
struct timeval tv;
int retval = ERROR_OK;
if(fd < 0)
{
fd = fileno(device);
if(fd < 0)
{
err_log_stderr("Failed to get fd!");
err_propagate(ERROR_IO);
}
}
FD_ZERO(&rfds);
FD_SET(fd,&rfds);
FD_ZERO(&wfds);
FD_SET(fd,&wfds);
// Set time waiting time to zero
tv.tv_sec = 0;
tv.tv_usec = 0;
// Check if we read/write without locking
retval = select(fd+1, \
(read_ok != NULL)?&rfds:NULL, \
(write_ok != NULL)?&wfds:NULL, \
NULL,&tv);
if(read_ok != NULL){
*read_ok = ((retval > 0) && FD_ISSET(fd,&rfds)) ? true:false;
}
if(write_ok != NULL)
*write_ok = ((retval >0) && FD_ISSET(fd,&wfds)) ? true:false;
if (retval < 0)
{
err_log_stderr("select() failed!");
err_propagate(ERROR_IO);
}
// else: retval == 0 <--> No data available
return ERROR_OK;
}
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
int main(int argc, char *argv[])
{
int ret = ERROR_OK;
int err_count = 0;
int rcv_err_count = 0;
bool msg_received = false;
char* device;
/* Para parsear el los mensajes se recorre el arreglo con un puntero
* a enteros de dos bytes.
*/
int16_t *ch_buff;
//char str[128]; // dbg
// check input arguments
if(argc<2)
{
err_log(HOW_TO);
return -1;
}
else device = argv[1];
struct timeval tv_in;
struct timeval tv_end;
struct timeval tv_diff;
#if DEBUG_TIMING_SBUSD
struct timeval tv_last;
#endif //#if DEBUG_TIMING_SBUSD
#if !PC_TEST
fd = open_port(device);
if (fd == -1)
{
return -1;
}
configure_port(fd);
ret = custom_baud(fd);
if (ret < 0)
{
err_log_stderr("custom_baud() failed!");
return ret;
}
#endif
/**
* Inherit priority from main.c for correct IPC.
*/
if(setpriority(PRIO_PROCESS, 0, -18) == -1) //requires being superuser
{
err_log_num("setpriority() failed!",errno);
return -1;
}
// Catch signals
prctl(PR_SET_PDEATHSIG, SIGHUP);
signal(SIGHUP, uquad_sig_handler);
signal(SIGINT, uquad_sig_handler);
signal(SIGQUIT, uquad_sig_handler);
#if PC_TEST
futaba_sbus_begin(); //para tiempo de start
#endif //PC_TEST
// Lleva a cero todos los canales y el mensaje sbus
futaba_sbus_set_channel(ROLL_CHANNEL, 1500); //init roll en cero
futaba_sbus_set_channel(PITCH_CHANNEL, 1500); //init pitch en cero
futaba_sbus_set_channel(YAW_CHANNEL, 1500); //init yaw en cero
futaba_sbus_set_channel(THROTTLE_CHANNEL, 950); //init throttle en minimo
futaba_sbus_set_channel(FLIGHTMODE_CHANNEL, 1500); //inint flight mode 2
futaba_sbus_update_msg();
sleep_ms(500); //Para ponerme a tiro con main
bool main_ready = false;
// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
// Loop
// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
for(;;)
{
gettimeofday(&tv_in,NULL);
//printf("errores: %d\n",err_count);//dbg
#if !PC_TEST
if (err_count > MAX_ERR_SBUSD)
{
err_log("error count exceded");
//err_count = 0;
quit();
}
#endif
ret = uquad_read(&rbuf);
if(ret == ERROR_OK)
{
if(!main_ready) main_ready = true;
//err_log("read ok!");
msg_received = true;
// Parse message. 2 bytes per channel.
ch_buff = (int16_t *)rbuf.mtext;
// send ack
ret = uquad_send_ack();
if(ret != ERROR_OK)
{
err_log("Failed to send ack!");
}
if (rcv_err_count > 0)
rcv_err_count = 0;
} else {
//err_log("Failed to read msg!");
msg_received = false;
rcv_err_count++;
if (main_ready && rcv_err_count > 3) {
err_count++;
rcv_err_count = 0;
}
}
if(msg_received)
{
futaba_sbus_set_channel(ROLL_CHANNEL, ch_buff[ROLL_CH_INDEX]);
futaba_sbus_set_channel(PITCH_CHANNEL, ch_buff[PITCH_CH_INDEX]);
futaba_sbus_set_channel(YAW_CHANNEL, ch_buff[YAW_CH_INDEX]);
futaba_sbus_set_channel(THROTTLE_CHANNEL, ch_buff[THROTTLE_CH_INDEX]);
//futaba_sbus_set_channel(7, ch_buff[FLIGHTMODE_CH_INDEX]); // flight mode no se modifica
// Comando para activar failsafe
if ( (ch_buff[FAILSAFE_CH_INDEX] == ACTIVATE_FAILSAFE) &&
(futaba_sbus_get_failsafe() == SBUS_SIGNAL_OK) )
futaba_sbus_set_failsafe(SBUS_SIGNAL_FAILSAFE);
// Comando para desactivar failsafe
if ( (ch_buff[FAILSAFE_CH_INDEX] == DEACTIVATE_FAILSAFE) &&
(futaba_sbus_get_failsafe() == SBUS_SIGNAL_FAILSAFE) )
futaba_sbus_set_failsafe(SBUS_SIGNAL_OK);
futaba_sbus_update_msg();
msg_received = false;
//print_sbus_data(); // dbg
}
#if !PC_TEST
ret = futaba_sbus_write_msg(fd);
if (ret < 0)
{
err_count++; // si fallo al enviar el mensaje se apagan los motores!!
}
else
{
/// This loop was fine
if(err_count > 0)
err_count--;
}
sleep_ms(5); //TODO revisar si hay que hacerlo siempre!!
#else
sleep_ms(5); //TODO revisar si hay que hacerlo siempre!!
#endif
// Escribe el mensaje a stdout - dbg
//convert_sbus_data(str);
//printf("%s",str);
/// Control de tiempo
gettimeofday(&tv_end,NULL);
ret = uquad_timeval_substract(&tv_diff, tv_end, tv_in);
if(ret > 0)
{
if(tv_diff.tv_usec < LOOP_T_US)
usleep(LOOP_T_US - (unsigned long)tv_diff.tv_usec);
#if DEBUG_TIMING_SBUSD
gettimeofday(&tv_end,NULL);
uquad_timeval_substract(&tv_diff, tv_end, tv_in);
printf("duracion loop sbusd (14ms): %lu\n",(unsigned long)tv_diff.tv_usec);
#endif
} else {
err_log("WARN: Absurd timing!");
err_count++; // si no cumplo el tiempo de loop falla la comunicacion sbus
}
//printf("rcv_err_count: %d\n", rcv_err_count);
//printf("err_count: %d\n", err_count);
} //for(;;)
return 0; //never gets here
}
int control_lqr(uquad_mat_t *K, uquad_mat_t *A, uquad_mat_t *B, uquad_mat_t *Q, uquad_mat_t *R)
{
int
err_time,
retval;
double norm;
uquad_mat_t *aux0 = NULL;
uquad_mat_t *aux1 = NULL;
uquad_mat_t *aux2 = NULL;
uquad_mat_t *aux3 = NULL;
uquad_mat_t *aux4 = NULL;
uquad_mat_t *aux5 = NULL;
uquad_mat_t *aux6 = NULL;
uquad_mat_t *aux7 = NULL;
uquad_mat_t *aux8 = NULL;
uquad_mat_t *aux9 = NULL;
uquad_mat_t *aux10 = NULL;
uquad_mat_t *aux11 = NULL;
uquad_mat_t *aux12 = NULL;
uquad_mat_t *aux13 = NULL;
uquad_mat_t *P = NULL;
uquad_mat_t *k = NULL;
uquad_mat_t *Bt = NULL;
struct timeval
tv_in,
tv_tmp,
tv_diff,
tv_out;
tv_out.tv_sec = 1;
tv_out.tv_usec = 0;
retval = gettimeofday(&tv_in,NULL);
if(retval < 0)
{
err_log_stderr("gettimeofday()");
retval = ERROR_TIMING;
cleanup_if(retval);
}
P = uquad_mat_alloc(Q->r,Q->c);
k = uquad_mat_alloc(B->c,B->r);
retval = uquad_mat_fill(K,1.0);
cleanup_if(retval);
retval = uquad_mat_copy(P,Q);
cleanup_if(retval);
retval = uquad_mat_copy(k,K);
cleanup_if(retval);
Bt = uquad_mat_alloc(B->c,B->r);
retval = uquad_mat_transpose(Bt,B);
cleanup_if(retval);
norm=1;
//For K=-(R+B'*P*B)^(-1)*B'*P*A
aux0 = uquad_mat_alloc(P->r,B->c);
aux1 = uquad_mat_alloc(R->r,R->c);
aux2 = uquad_mat_alloc(R->r,R->c);
aux3 = uquad_mat_alloc(R->r,R->c);
aux4 = uquad_mat_alloc(R->r,2*R->c);
aux5 = uquad_mat_alloc(R->r,B->r);
aux6 = uquad_mat_alloc(R->r,P->c);
//Performs P=Q+K'*R*K+(A+B*K)'*P*(A+B*K)
//K'*R*K
aux7 = uquad_mat_alloc(K->c,K->r);
aux8 = uquad_mat_alloc(K->c,R->c);
aux9 = uquad_mat_alloc(K->c,K->c);
//A+B*K
aux10 = uquad_mat_alloc(B->r,K->c);
aux11 = uquad_mat_alloc(K->c,B->r);
aux12 = uquad_mat_alloc(K->c,P->c);
aux13 = uquad_mat_alloc(K->r,K->c);
while (norm>CTRL_LQR_TH)
{
retval = uquad_mat_copy(k,K);
cleanup_if(retval);
//Performs K=-(R+B'*P*B)^(-1)*B'*P*A;
retval = uquad_mat_prod(aux0,P,B);
cleanup_if(retval);
retval = uquad_mat_prod(aux1,Bt,aux0);
cleanup_if(retval);
retval = uquad_mat_add(aux1,aux1,R);
cleanup_if(retval);
retval = uquad_mat_inv(aux2,aux1,aux3,aux4);
cleanup_if(retval);
retval = uquad_mat_prod(aux5,aux2,Bt);
cleanup_if(retval);
retval = uquad_mat_prod(aux6,aux5,P);
cleanup_if(retval);
retval = uquad_mat_prod(aux5,aux6,A);
cleanup_if(retval);
retval = uquad_mat_scalar_mul(K,aux5,-1);
cleanup_if(retval);
//Performs P=Q+K'*R*K+(A+B*K)'*P*(A+B*K);
//K'*R*K
retval = uquad_mat_transpose(aux7,K);
cleanup_if(retval);
retval = uquad_mat_prod(aux8,aux7,R);
cleanup_if(retval);
retval = uquad_mat_prod(aux9,aux8,K);
cleanup_if(retval);
//A+B*K
retval = uquad_mat_prod(aux10,B,K);
cleanup_if(retval);
retval = uquad_mat_add(aux10,aux10,A);
cleanup_if(retval);
retval = uquad_mat_transpose(aux11,aux10);
cleanup_if(retval);
retval = uquad_mat_prod(aux12,aux11,P);
cleanup_if(retval);
retval = uquad_mat_prod(aux11,aux12,aux10);
cleanup_if(retval);
retval = uquad_mat_add(P,Q,aux9);
cleanup_if(retval);
retval = uquad_mat_add(P,P,aux11);
cleanup_if(retval);
retval = uquad_mat_sub(aux13,K,k);
cleanup_if(retval);
norm = uquad_mat_norm(aux13);
/// Check timeout
err_time = gettimeofday(&tv_tmp, NULL);
if(err_time < 0)
{
retval = ERROR_TIMING;
err_log_stderr("gettimeofday()");
cleanup_if(retval);
}
err_time = uquad_timeval_substract(&tv_diff, tv_tmp, tv_in);
if(err_time < 0)
{
retval = ERROR_TIMING;
cleanup_log_if(retval,"Absurd timing!");
}
err_time = uquad_timeval_substract(&tv_diff, tv_diff, tv_out);
if(err_time >= 0)
{
retval = ERROR_FAIL;
cleanup_log_if(retval,"ERR: Timed out!");
}
}
retval = uquad_mat_scalar_mul(K,K,-1);
cleanup_if(retval);
cleanup:
uquad_mat_free(aux0);
uquad_mat_free(aux1);
uquad_mat_free(aux2);
uquad_mat_free(aux3);
uquad_mat_free(aux4);
uquad_mat_free(aux5);
uquad_mat_free(aux6);
uquad_mat_free(aux7);
uquad_mat_free(aux8);
uquad_mat_free(aux9);
uquad_mat_free(aux10);
uquad_mat_free(aux11);
uquad_mat_free(aux12);
uquad_mat_free(k);
uquad_mat_free(P);
return retval;
}
int main(int argc, char *argv[])
{
// Catch signals
signal(SIGINT, uquad_sig_handler);
signal(SIGQUIT, uquad_sig_handler);
int
i,
retval;
FILE *file;
uquad_mat_t *x, *w;
uquad_bool_t ctrl_outdated = false;
double w_hover;
if(argc < 2)
{
err_log_str("Invalid arguments!",USAGE)
quit();
}
else
{
file = fopen(argv[1],"r");
if(file == NULL)
{
err_log_stderr("Failed to open log!");
quit();
}
}
x = uquad_mat_alloc(STATE_COUNT,1);
w = uquad_mat_alloc(LENGTH_INPUT,1);
if(x == NULL || w == NULL)
{
quit_log_if(ERROR_MALLOC,"Failed to allocate tmp mem!");
}
/**
* The following should be done by calling mot_update_w_hover(), this
* is just a test program.
*/
retval = uquad_solve_pol2(&w_hover, NULL, F_B1, F_B2, -GRAVITY*MASA_DEFAULT/4.0);
quit_log_if(retval, "Failed to get w_hover!");
for(i=0; i<LENGTH_INPUT; ++i)
w->m_full[i] = w_hover;
retval = uquad_mat_zeros(x);
quit_if(retval);
ctrl = control_init();
if(ctrl == NULL)
{
quit_if(ERROR_FAIL);
}
pp = pp_init();
if(pp == NULL)
{
quit_if(ERROR_FAIL);
}
retval = pp_new_setpoint(pp, x, w);
quit_if(retval);
retval = control_update_K(ctrl, pp, MASA_DEFAULT);
quit_log_if(retval, "Failed to update control matrix! Aborting...");
for(;;)
{
retval = uquad_mat_load(x,file);
if(retval != ERROR_OK)
{
quit_log_if(retval, "End of log?");
}
#warning "w_hover should use mot_control.h!!"
retval = pp_update_setpoint(pp, x, w_hover, &ctrl_outdated);
quit_if(retval);
if(ctrl_outdated)
{
retval = control_update_K(ctrl, pp, MASA_DEFAULT);
quit_log_if(retval, "Failed to update control matrix! Aborting...");
retval = control_dump(ctrl, NULL);
quit_log_if(retval, "Failed to dump new control matrix! Aborting...");
}
retval = control(ctrl, w, x, pp->sp, TS_DEFAULT_US);
quit_if(retval);
ctrl_outdated = false;
}
// Never gets here
quit();
}