int main(int argc, char *argv[])
{
int
portno,
retval;
uquad_bool_t
udp = false;
if (argc > 4)
{
err_check(ERROR_INVALID_ARG, USAGE);
}
if (argc > 3)
{
udp = atoi(argv[3]);
}
if (argc > 2)
{
portno = atoi(argv[2]);
}
else
{
err_log_num("Using default port:", CHECK_NET_PORT);
portno = CHECK_NET_PORT;
}
if (argc <= 1)
{
err_log_str("Using default checknet IP:", CHECK_NET_SERVER_IP);
}
retval = uquad_check_net_client((argc > 1)?argv[1]:CHECK_NET_SERVER_IP,
portno,
udp);
if(retval < 0)
{
err_check(retval, "client() failed!");
}
else
{
child_pid = retval;
retval = ERROR_OK;
err_log_num("Client running! Child pid:",child_pid);
for(;;)
sleep(INT_MAX);
}
return retval;
}
int mot_set_vel_rads(uquad_mot_t *mot, uquad_mat_t *w, uquad_bool_t force)
{
int i,retval;
int itmp;
for(i=0; i < MOT_C; ++i)
{
if(!force)
{
if(w->m_full[i] < mot->w_min)
{
/**
* Setting speed to less than MOT_W_IDLE could
* cause motors to stop.
*
*/
#ifdef DEBUG_MOT
err_log_num("WARN:w out of range, setting min for motor:",i);
#endif // DEBUG_MOT
w->m_full[i] = mot->w_min;
}
else if (w->m_full[i] > MOT_W_MAX)
{
/**
* Setting speed to more than MOT_MAX_W could
* damage battery.
*
*/
#ifdef DEBUG_MOT
err_log_num("WARN:w out of range, setting max for motor:",i);
#endif // DEBUG_MOT
w->m_full[i] = MOT_W_MAX;
}
}
retval = mot_rad2i2c(w->m_full[i],&itmp);
err_propagate(retval);
mot->i2c_target[i] = (uint8_t)itmp;
}
retval = mot_send(mot, w->m_full);
err_propagate(retval);
return retval;
};
void uquad_conn_lost_handler(int signal_num)
{
pid_t p;
int status;
p = waitpid(-1, &status, WNOHANG);
if(p == child_pid)
{
err_log_num("Client died, caught signal",signal_num);
exit(0);
}
}
void uquad_sig_handler(int signal_num)
{
// Si se murio el demonio sbus termino el programa
if (signal_num == SIGCHLD)
{
pid_t p;
int status;
p = waitpid(-1, &status, WNOHANG);
if(p == sbusd_child_pid)
{
err_log_num("WARN: sbusd died! sig num:", signal_num);
quit(1); //exit sin cerrar sbusd
#if !SIMULATE_GPS
} else if(p == gpsd_child_pid) {
err_log_num("WARN: gpsd died! sig num:", signal_num);
quit(0); //exit sin cerrar gpsd
#endif //!SIMULATE_GPS
#if !DISABLE_UAVTALK
} else if(p == uavtalk_child_pid) {
err_log_num("WARN: uavtalk_parser died! sig num:", signal_num);
quit(3); //exit sin cerrar gpsd
#endif //!DISABLE_UAVTALK
} else {
err_log_num("SIGCHLD desconocido, return:", signal_num);
//quit(0);
return;
}
}
// bloqueo SIGCHLD si entre por SIGINT para no entrar 2 veces
if (sigprocmask(SIG_BLOCK, &mask, &orig_mask) < 0) {
perror ("sigprocmask");
}
// Si se capturo SIGINT o SIGQUIT termino el programa
err_log_num("Caught signal:",signal_num);
quit(2);
}
void read_from_stdin(void)
{
int retval = fread(tmp_buff,sizeof(unsigned char),1,stdin); //TODO corregir que queda algo por leer en el buffer?
if(retval < 0)
{
//log_n_jump(ERROR_READ, end_stdin,"No user input detected!");
err_log_num("No user input detected!",ERROR_READ);
return;
}
//retval = 0;
switch(tmp_buff[0])
{
case 'S':
//ch_buff[THROTTLE_CH_INDEX] = throttle_inicial; //No va ahora
//set_alt_zero(double alt_measured);
takeoff = 1; //true
puts("Despegando!");
control_status = STARTED;
break;
case 'P':
//ch_buff[THROTTLE_CH_INDEX] = THROTTLE_NEUTRAL; //No va ahora
takeoff = -1; //aterrizando
puts("Aterrizando");
//control_status = STOPPED;
break;
case 'F':
puts("WARN: Failsafe set");
ch_buff[FAILSAFE_CH_INDEX] = ACTIVATE_FAILSAFE;
break;
case 'f':
puts("WARN: Failsafe clear");
ch_buff[FAILSAFE_CH_INDEX] = DEACTIVATE_FAILSAFE;
break;
case 'b':
ch_buff[ROLL_CH_INDEX] = ROLL_NEUTRAL;
ch_buff[PITCH_CH_INDEX] = PITCH_NEUTRAL;
ch_buff[YAW_CH_INDEX] = YAW_NEUTRAL;
ch_buff[THROTTLE_CH_INDEX] = THROTTLE_NEUTRAL;
ch_buff[FLIGHTMODE_CH_INDEX] = FLIGHT_MODE_2; //neutral value
puts("Seteando valor neutro");
break;
case 'A':
#if !FAKE_YAW
//yaw_zero = act.yaw;
set_yaw_zero(act.yaw);
#endif
ch_buff[ROLL_CH_INDEX] = ROLL_NEUTRAL;
ch_buff[PITCH_CH_INDEX] = PITCH_NEUTRAL;
ch_buff[YAW_CH_INDEX] = YAW_ARM;
ch_buff[THROTTLE_CH_INDEX] = THROTTLE_ARM;
puts("Armando...");
break;
case 'D':
ch_buff[ROLL_CH_INDEX] = ROLL_NEUTRAL;
ch_buff[PITCH_CH_INDEX] = PITCH_NEUTRAL;
ch_buff[YAW_CH_INDEX] = YAW_DISARM;
ch_buff[THROTTLE_CH_INDEX] = THROTTLE_DISARM;
puts("Desarmando...");
break;
case '1':
puts("Bajando 10cm");
h_d = h_d - 0.1;
break;
case '3':
puts("Subiendo 10cm");
h_d = h_d + 0.1;
break;
#ifdef SETANDO_CC3D
// Para setear maximos y minimos en CC3D
case 'M':
ch_buff[ROLL_CH_INDEX] = MAX_COMMAND;
ch_buff[PITCH_CH_INDEX] = MAX_COMMAND;
ch_buff[YAW_CH_INDEX] = MAX_COMMAND;
ch_buff[THROTTLE_CH_INDEX] = MAX_COMMAND;
ch_buff[FLIGHTMODE_CH_INDEX] = MAX_COMMAND;
puts("Seteando maximo valor");
break;
case 'm':
ch_buff[ROLL_CH_INDEX] = MIN_COMMAND;
ch_buff[PITCH_CH_INDEX] = MIN_COMMAND;
ch_buff[YAW_CH_INDEX] = MIN_COMMAND;
ch_buff[THROTTLE_CH_INDEX] = MIN_THROTTLE; // Se necesecita que sea distino por el armado/desarmado
ch_buff[FLIGHTMODE_CH_INDEX] = MIN_COMMAND;
puts("Seteando minimo valor");
break;
#endif
default:
#if DEBUG
puts("comando invalido");
#endif
break;
} //switch(tmp_buff[0])
fflush( stdin );
return;
}
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
}
void uquad_sig_handler(int signal_num){
err_log_num("[Client] Caught signal: ",signal_num);
quit();
}