int cmd_shortlog(int argc, const char **argv, const char *prefix)
{
static struct shortlog log;
static struct rev_info rev;
int nongit = !startup_info->have_repository;
static const struct option options[] = {
OPT_BOOL('n', "numbered", &log.sort_by_number,
N_("sort output according to the number of commits per author")),
OPT_BOOL('s', "summary", &log.summary,
N_("Suppress commit descriptions, only provides commit count")),
OPT_BOOL('e', "email", &log.email,
N_("Show the email address of each author")),
{ OPTION_CALLBACK, 'w', NULL, &log, N_("w[,i1[,i2]]"),
N_("Linewrap output"), PARSE_OPT_OPTARG, &parse_wrap_args },
OPT_END(),
};
struct parse_opt_ctx_t ctx;
git_config(git_default_config, NULL);
shortlog_init(&log);
init_revisions(&rev, prefix);
parse_options_start(&ctx, argc, argv, prefix, options,
PARSE_OPT_KEEP_DASHDASH | PARSE_OPT_KEEP_ARGV0);
for (;;) {
switch (parse_options_step(&ctx, options, shortlog_usage)) {
case PARSE_OPT_HELP:
exit(129);
case PARSE_OPT_DONE:
goto parse_done;
}
parse_revision_opt(&rev, &ctx, options, shortlog_usage);
}
parse_done:
argc = parse_options_end(&ctx);
if (setup_revisions(argc, argv, &rev, NULL) != 1) {
error(_("unrecognized argument: %s"), argv[1]);
usage_with_options(shortlog_usage, options);
}
log.user_format = rev.commit_format == CMIT_FMT_USERFORMAT;
log.abbrev = rev.abbrev;
/* assume HEAD if from a tty */
if (!nongit && !rev.pending.nr && isatty(0))
add_head_to_pending(&rev);
if (rev.pending.nr == 0) {
if (isatty(0))
fprintf(stderr, _("(reading log message from standard input)\n"));
read_from_stdin(&log);
}
else
get_from_rev(&rev, &log);
shortlog_output(&log);
return 0;
}
int main(int argc, char **argv)
{
read_from_stdin();
find_best_prices();
print_best_prices();
return 0;
}
int main(int argc, char **argv)
{
make_atan2_table();
init_fec();
read_from_stdin();
return 0;
}
int main(int argc, char **argv){
char *str;
if(argc == 4){
sscanf(argv[1],"%d",&s_size);
sscanf(argv[2],"%d",&g_size);
sscanf(argv[3],"%d",&offset);
}
str = malloc(s_size*sizeof(char));
read_from_stdin();
return(0);
}
int main(int argc, char **argv)
{
gettimeofday(&t[0], NULL);
read_from_stdin();
gettimeofday(&t[5], NULL);
find_best_prices();
gettimeofday(&t[6], NULL);
print_best_prices();
gettimeofday(&t[7], NULL);
report_branch();
report_time();
return 0;
}
int main(int argc, char **argv){
int count;
if(argc == 1){
printf("<table>\n");
print_table_start();
count = read_from_stdin();
print_table_end();
printf("</table>\n");
printf("<br>\n");
printf("%dunits tested\n",count);
}
return(0);
}
static int read_from_any_channel(inport **inp)
{
int i, nelem, error;
fd_set read_template;
retry:
FD_ZERO(&read_template);
for(i = 0; i < ninports; i++){
if(inportset[i].in >= 0 && !inportset[i].suspended)
FD_SET(inportset[i].in,&read_template);
}
/* TBmsg("read_from_any_channel, before select\n"); */
if((error = select(FD_SETSIZE, &read_template,
NULL, NULL, NULL)) >= 0){
for(i = 0; i < ninports; i++){
if(inportset[i].in >= 0 &&
FD_ISSET(inportset[i].in,&read_template)) {
/* TBmsg("read_from_any_channel, data on port %d\n", inportset[i].in); */
if(inportset[i].in == 0){
nelem = read_from_stdin();
} else if(inportset[i].term_port == TBfalse){
*inp = &inportset[i];
return 0;
} else {
nelem = multi_read(inportset[i].in);
}
if(nelem == 0){
err_warn("lost connection with ToolBus");
exit(-1);
}
if(nelem < 0){
err_sys_fatal("read failed"); /**************/
goto retry;
}
*inp = &inportset[i];
return nelem;
}
}
} else {
if(errno != EINTR)
err_sys_warn("select failed");
goto retry;
}
return TB_ERROR;
}
int swaynag_parse_options(int argc, char **argv, struct swaynag *swaynag,
list_t *types, struct swaynag_type *type, char **config, bool *debug) {
enum type_options {
TO_COLOR_BACKGROUND = 256,
TO_COLOR_BORDER,
TO_COLOR_BORDER_BOTTOM,
TO_COLOR_BUTTON,
TO_COLOR_TEXT,
TO_THICK_BAR_BORDER,
TO_PADDING_MESSAGE,
TO_THICK_DET_BORDER,
TO_THICK_BTN_BORDER,
TO_GAP_BTN,
TO_GAP_BTN_DISMISS,
TO_MARGIN_BTN_RIGHT,
TO_PADDING_BTN,
};
static struct option opts[] = {
{"button", required_argument, NULL, 'b'},
{"button-no-terminal", required_argument, NULL, 'B'},
{"config", required_argument, NULL, 'c'},
{"debug", no_argument, NULL, 'd'},
{"edge", required_argument, NULL, 'e'},
{"font", required_argument, NULL, 'f'},
{"help", no_argument, NULL, 'h'},
{"detailed-message", no_argument, NULL, 'l'},
{"detailed-button", required_argument, NULL, 'L'},
{"message", required_argument, NULL, 'm'},
{"output", required_argument, NULL, 'o'},
{"dismiss-button", required_argument, NULL, 's'},
{"type", required_argument, NULL, 't'},
{"version", no_argument, NULL, 'v'},
{"background", required_argument, NULL, TO_COLOR_BACKGROUND},
{"border", required_argument, NULL, TO_COLOR_BORDER},
{"border-bottom", required_argument, NULL, TO_COLOR_BORDER_BOTTOM},
{"button-background", required_argument, NULL, TO_COLOR_BUTTON},
{"text", required_argument, NULL, TO_COLOR_TEXT},
{"border-bottom-size", required_argument, NULL, TO_THICK_BAR_BORDER},
{"message-padding", required_argument, NULL, TO_PADDING_MESSAGE},
{"details-border-size", required_argument, NULL, TO_THICK_DET_BORDER},
{"button-border-size", required_argument, NULL, TO_THICK_BTN_BORDER},
{"button-gap", required_argument, NULL, TO_GAP_BTN},
{"button-dismiss-gap", required_argument, NULL, TO_GAP_BTN_DISMISS},
{"button-margin-right", required_argument, NULL, TO_MARGIN_BTN_RIGHT},
{"button-padding", required_argument, NULL, TO_PADDING_BTN},
{0, 0, 0, 0}
};
const char *usage =
"Usage: swaynag [options...]\n"
"\n"
" -b, --button <text> <action> Create a button with text that "
"executes action in a terminal when pressed. Multiple buttons can "
"be defined.\n"
" -B, --button-no-terminal <text> <action> Like --button, but does"
"not run the action in a terminal.\n"
" -c, --config <path> Path to config file.\n"
" -d, --debug Enable debugging.\n"
" -e, --edge top|bottom Set the edge to use.\n"
" -f, --font <font> Set the font to use.\n"
" -h, --help Show help message and quit.\n"
" -l, --detailed-message Read a detailed message from stdin.\n"
" -L, --detailed-button <text> Set the text of the detail button.\n"
" -m, --message <msg> Set the message text.\n"
" -o, --output <output> Set the output to use.\n"
" -s, --dismiss-button <text> Set the dismiss button text.\n"
" -t, --type <type> Set the message type.\n"
" -v, --version Show the version number and quit.\n"
"\n"
"The following appearance options can also be given:\n"
" --background RRGGBB[AA] Background color.\n"
" --border RRGGBB[AA] Border color.\n"
" --border-bottom RRGGBB[AA] Bottom border color.\n"
" --button-background RRGGBB[AA] Button background color.\n"
" --text RRGGBB[AA] Text color.\n"
" --border-bottom-size size Thickness of the bar border.\n"
" --message-padding padding Padding for the message.\n"
" --details-border-size size Thickness for the details border.\n"
" --button-border-size size Thickness for the button border.\n"
" --button-gap gap Size of the gap between buttons\n"
" --button-dismiss-gap gap Size of the gap for dismiss button.\n"
" --button-margin-right margin Margin from dismiss button to edge.\n"
" --button-padding padding Padding for the button text.\n";
optind = 1;
while (1) {
int c = getopt_long(argc, argv, "b:B:c:de:f:hlL:m:o:s:t:v", opts, NULL);
if (c == -1) {
break;
}
switch (c) {
case 'b': // Button
case 'B': // Button (No Terminal)
if (swaynag) {
if (optind >= argc) {
fprintf(stderr, "Missing action for button %s\n", optarg);
return EXIT_FAILURE;
}
struct swaynag_button *button;
button = calloc(sizeof(struct swaynag_button), 1);
button->text = strdup(optarg);
button->type = SWAYNAG_ACTION_COMMAND;
button->action = strdup(argv[optind]);
button->terminal = c == 'b';
list_add(swaynag->buttons, button);
}
optind++;
break;
case 'c': // Config
if (config) {
*config = strdup(optarg);
}
break;
case 'd': // Debug
if (debug) {
*debug = true;
}
break;
case 'e': // Edge
if (type) {
if (strcmp(optarg, "top") == 0) {
type->anchors = ZWLR_LAYER_SURFACE_V1_ANCHOR_TOP
| ZWLR_LAYER_SURFACE_V1_ANCHOR_LEFT
| ZWLR_LAYER_SURFACE_V1_ANCHOR_RIGHT;
} else if (strcmp(optarg, "bottom") == 0) {
type->anchors = ZWLR_LAYER_SURFACE_V1_ANCHOR_BOTTOM
| ZWLR_LAYER_SURFACE_V1_ANCHOR_LEFT
| ZWLR_LAYER_SURFACE_V1_ANCHOR_RIGHT;
} else {
fprintf(stderr, "Invalid edge: %s\n", optarg);
return EXIT_FAILURE;
}
}
break;
case 'f': // Font
if (type) {
free(type->font);
type->font = strdup(optarg);
}
break;
case 'l': // Detailed Message
if (swaynag) {
free(swaynag->details.message);
swaynag->details.message = read_from_stdin();
swaynag->details.button_up.text = strdup("▲");
swaynag->details.button_down.text = strdup("▼");
}
break;
case 'L': // Detailed Button Text
if (swaynag) {
free(swaynag->details.button_details->text);
swaynag->details.button_details->text = strdup(optarg);
}
break;
case 'm': // Message
if (swaynag) {
free(swaynag->message);
swaynag->message = strdup(optarg);
}
break;
case 'o': // Output
if (type) {
free(type->output);
type->output = strdup(optarg);
}
break;
case 's': // Dismiss Button Text
if (swaynag) {
struct swaynag_button *button_close;
button_close = swaynag->buttons->items[0];
free(button_close->text);
button_close->text = strdup(optarg);
}
break;
case 't': // Type
if (swaynag) {
swaynag->type = swaynag_type_get(types, optarg);
if (!swaynag->type) {
fprintf(stderr, "Unknown type %s\n", optarg);
return EXIT_FAILURE;
}
}
break;
case 'v': // Version
fprintf(stdout, "swaynag version " SWAY_VERSION "\n");
return -1;
case TO_COLOR_BACKGROUND: // Background color
if (type) {
type->background = parse_color(optarg);
}
break;
case TO_COLOR_BORDER: // Border color
if (type) {
type->border = parse_color(optarg);
}
break;
case TO_COLOR_BORDER_BOTTOM: // Bottom border color
if (type) {
type->border_bottom = parse_color(optarg);
}
break;
case TO_COLOR_BUTTON: // Button background color
if (type) {
type->button_background = parse_color(optarg);
}
break;
case TO_COLOR_TEXT: // Text color
if (type) {
type->text = parse_color(optarg);
}
break;
case TO_THICK_BAR_BORDER: // Bottom border thickness
if (type) {
type->bar_border_thickness = strtol(optarg, NULL, 0);
}
break;
case TO_PADDING_MESSAGE: // Message padding
if (type) {
type->message_padding = strtol(optarg, NULL, 0);
}
break;
case TO_THICK_DET_BORDER: // Details border thickness
if (type) {
type->details_border_thickness = strtol(optarg, NULL, 0);
}
break;
case TO_THICK_BTN_BORDER: // Button border thickness
if (type) {
type->button_border_thickness = strtol(optarg, NULL, 0);
}
break;
case TO_GAP_BTN: // Gap between buttons
if (type) {
type->button_gap = strtol(optarg, NULL, 0);
}
break;
case TO_GAP_BTN_DISMISS: // Gap between dismiss button
if (type) {
type->button_gap_close = strtol(optarg, NULL, 0);
}
break;
case TO_MARGIN_BTN_RIGHT: // Margin on the right side of button area
if (type) {
type->button_margin_right = strtol(optarg, NULL, 0);
}
break;
case TO_PADDING_BTN: // Padding for the button text
if (type) {
type->button_padding = strtol(optarg, NULL, 0);
}
break;
default: // Help or unknown flag
fprintf(c == 'h' ? stdout : stderr, "%s", usage);
return -1;
}
}
return 0;
}
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 c;
int32_t verb_lvl;
ARGV = argv;
ARGC = argc;
program_name = argv[0];
char opt_string[] = "hp:PrR:sv?";
//initalize global arguments
arguments_g.verbosity = 0;
arguments_g.filter_mode = 0;
arguments_g.stats = 0;
arguments_g.files = 0;
arguments_g.dcap = 0;
arguments_g.dcap_file = 0;
arguments_g.write_file_timestamp = 0;
DBG("dnsflow header len %u\n", (unsigned int)sizeof(struct dnsflow_hdr));
DBG("dcap header len %u\n", (unsigned int)sizeof(struct dcap_header));
DBG("dnsflow_set_hdr %u\n", (unsigned int) sizeof(struct dnsflow_set_hdr));
DBG("struct tm32 %u\n", (unsigned int) sizeof(struct tm32));
//Get options
while ((c = getopt(argc, argv, opt_string)) != -1) {
switch (c) {
case 'p': //verbosity level
verb_lvl = atoi(optarg);
arguments_g.verbosity = verb_lvl;
if(verb_lvl > 2 || verb_lvl < 0)
{
printf("Using default verbosity\n");
arguments_g.verbosity = 0;
}
if(arguments_g.verbosity == 2)
arguments_g.stats = 1;
break;
case 'P': //pretty print
arguments_g.verbosity = 3;
break;
case 'r': //reading from pcap file NOTE: could be flag too
arguments_g.files = 1;
arguments_g.filter_mode = 1;
break;
case 'R': //read from dcap file -- no glob expansion supported
arguments_g.dcap = 1;
arguments_g.dcap_file = optarg;
break;
case 's' :
arguments_g.stats = 1;
break;
case 'v':
//print version
printf("Dnsflow-print version %d\n", DNSFLOW_PRINT_VERSION);
exit(1);
break;
case 'h': //intentional fall through
case '?':
case '-':
default:
usage();
/* NOTREACHED */
}
}
//update where argv and argc point
//NOTE: Get opt moves files names, or invalid arguments to the end of the
//argv array. After these lines, argv and argc will point to the filesnames
//or invalide arguments
argc -= optind;
argv += optind;
process_globals();
if(arguments_g.files)
{
DBG("%s\n", "checking for pcap files");
//check if std in
char * ptr = strtok(argv[0], WHITESPACE_DELIMS);
if(*ptr == '-')
{
return read_from_stdin();
}
int32_t ret = process_from_pcapfiles(argv, argc);
return ret;
}
else if(arguments_g.dcap)
{
//read from dcap file given by -R flag
//check if stdin
DBG("%s\n", "checking for dcap files");
if(*arguments_g.dcap_file == '-')
{
DBG("%s\n", "hi");
return read_dcap_filestream(stdin);
}
DBG("%s\n", "h");
print_dcap_file(arguments_g.dcap_file);
return 0;
}
//process dnsflow data from networks -- default action
int socketfd = get_dnsflow_socketfd(DNSFLOW_PORT, 0, arguments_g.verbosity);
listen_for_dnsflow_pkt(socketfd, parse_dnsflow_packet);
return 0;
}
/* To reduce redundant error handling code [calls to abort_connection()]
* most of the function is build around conditions that will assign the error
* code to @state if anything goes wrong. The rest of the function will then just
* do the necessary cleanups. If all works out we end up with @state being S_OK
* resulting in a cache entry being created with the fragment data generated by
* either reading the file content or listing a directory. */
void
file_protocol_handler(struct connection *connection)
{
unsigned char *redirect_location = NULL;
struct string page, name;
struct connection_state state;
int set_dir_content_type = 0;
if (get_cmd_opt_bool((const unsigned char *)"anonymous")) {
if (strcmp((const char *)connection->uri->string, "file:///dev/stdin")
|| isatty(STDIN_FILENO)) {
abort_connection(connection,
connection_state(S_FILE_ANONYMOUS));
return;
}
}
#ifdef CONFIG_CGI
if (!execute_cgi(connection)) return;
#endif /* CONFIG_CGI */
/* Treat /dev/stdin in special way */
if (!strcmp((const char *)connection->uri->string, "file:///dev/stdin")) {
int fd = open("/dev/stdin", O_RDONLY);
if (fd == -1) {
abort_connection(connection, connection_state(-errno));
return;
}
set_nonblocking_fd(fd);
if (!init_http_connection_info(connection, 1, 0, 1)) {
abort_connection(connection, connection_state(S_OUT_OF_MEM));
close(fd);
return;
}
connection->socket->fd = fd;
connection->data_socket->fd = -1;
read_from_stdin(connection);
return;
}
/* This function works on already simplified file-scheme URI pre-chewed
* by transform_file_url(). By now, the function contains no hostname
* part anymore, possibly relative path is converted to an absolute one
* and uri->data is just the final path to file/dir we should try to
* show. */
if (!init_string(&name)
|| !add_uri_to_string(&name, connection->uri, URI_PATH)) {
done_string(&name);
abort_connection(connection, connection_state(S_OUT_OF_MEM));
return;
}
decode_uri_string(&name);
/* In Win32, file_is_dir seems to always return 0 if the name
* ends with a directory separator. */
if ((name.length > 0 && dir_sep(name.source[name.length - 1]))
|| file_is_dir(name.source)) {
/* In order for global history and directory listing to
* function properly the directory url must end with a
* directory separator. */
if (name.source[0] && !dir_sep(name.source[name.length - 1])) {
redirect_location = (unsigned char *)STRING_DIR_SEP;
state = connection_state(S_OK);
} else {
state = list_directory(connection, name.source, &page);
set_dir_content_type = 1;
}
} else {
state = read_encoded_file(&name, &page);
/* FIXME: If state is now S_ENCODE_ERROR we should try loading
* the file undecoded. --jonas */
}
done_string(&name);
if (is_in_state(state, S_OK)) {
struct cache_entry *cached;
/* Try to add fragment data to the connection cache if either
* file reading or directory listing worked out ok. */
cached = connection->cached = get_cache_entry(connection->uri);
if (!connection->cached) {
if (!redirect_location) done_string(&page);
state = connection_state(S_OUT_OF_MEM);
} else if (redirect_location) {
if (!redirect_cache(cached, redirect_location, 1, 0))
state = connection_state(S_OUT_OF_MEM);
} else {
add_fragment(cached, 0, page.source, page.length);
connection->from += page.length;
if (!cached->head && set_dir_content_type) {
unsigned char *head;
/* If the system charset somehow
* changes after the directory listing
* has been generated, it should be
* parsed with the original charset. */
head = straconcat((const unsigned char *)"\r\nContent-Type: text/html; charset=",
get_cp_mime_name(get_cp_index((const unsigned char *)"System")),
"\r\n", (unsigned char *) NULL);
/* Not so gracefully handle failed memory
* allocation. */
if (!head)
state = connection_state(S_OUT_OF_MEM);
/* Setup directory listing for viewing. */
mem_free_set(&cached->head, head);
}
done_string(&page);
}
}
abort_connection(connection, state);
}
