static void rs_interruptRx(int irq, void *dev_id, struct pt_regs *regs)
{
struct s3c3410_serial *info = &s3c3410_info;
struct tty_struct *tty = info->tty;
unsigned int count;
volatile u_int8_t status, fifo_status;
if (!info || !tty || (!(info->flags & S_INITIALIZED)))
return;
if ((tty->flip.count + RX_FIFO_DEPTH) >= TTY_FLIPBUF_SIZE)
queue_task_irq_off(&tty->flip.tqueue, &tq_timer);
count = RX_FIFO_DEPTH;
do {
status = inb(S3C3410X_UART_BASE+S3C3410X_USTAT);
if (!(status & USTAT_RFDR))
break;
/* check all error flags and accept data if valid */
fifo_status = inb(S3C3410X_UART_BASE+S3C3410X_UFSTAT);
if (!(status & USTAT_ERROR) && !(fifo_status & UFSTAT_ERROR))
*tty->flip.flag_buf_ptr = TTY_NORMAL;
else {
if (fifo_status & UFSTAT_RFF) {
*tty->flip.flag_buf_ptr = TTY_NORMAL;
handle_status (info, UFSTAT_RFF);
}
if (status & USTAT_OE) {
*tty->flip.flag_buf_ptr = TTY_OVERRUN;
handle_status (info, USTAT_OE);
}
if (status & USTAT_BD) {
*tty->flip.flag_buf_ptr = TTY_BREAK;
handle_status (info, USTAT_BD);
}
if (status & USTAT_PE) {
*tty->flip.flag_buf_ptr = TTY_PARITY;
handle_status (info, USTAT_PE);
}
if (status & USTAT_FE) {
*tty->flip.flag_buf_ptr = TTY_FRAME;
handle_status (info, USTAT_FE);
}
}
*tty->flip.char_buf_ptr++ =
inb(S3C3410X_UART_BASE+S3C3410X_URXH_B);
tty->flip.flag_buf_ptr++;
tty->flip.count++;
} while ((--count > 0) && !(status & USTAT_ERROR) &&
!(fifo_status & UFSTAT_ERROR) );
// if (fifo_status & (U_RFOV | U_E_RxTO))
// handle_status (info, (U_RFOV | U_E_RxTO));
queue_task_irq_off(&tty->flip.tqueue, &tq_timer);
}
/* Callback for dht public key changes. */
static void dht_pk_callback(void *object, int32_t number, const uint8_t *dht_public_key, void *userdata)
{
Friend_Connections *const fr_c = (Friend_Connections *)object;
Friend_Conn *const friend_con = get_conn(fr_c, number);
if (!friend_con) {
return;
}
if (public_key_cmp(friend_con->dht_temp_pk, dht_public_key) == 0) {
return;
}
change_dht_pk(fr_c, number, dht_public_key);
/* if pk changed, create a new connection.*/
if (friend_con->crypt_connection_id != -1) {
crypto_kill(fr_c->net_crypto, friend_con->crypt_connection_id);
friend_con->crypt_connection_id = -1;
handle_status(object, number, 0, userdata); /* Going offline. */
}
friend_new_connection(fr_c, number);
onion_set_friend_DHT_pubkey(fr_c->onion_c, friend_con->onion_friendnum, dht_public_key);
}
static void
handle_command(int argc, char **argv)
{
char *command = argv[0];
argc -= 1;
argv += 1;
if (!strcmp(command, "ping"))
handle_ping(argc, argv);
else if (!strcmp(command, "add-ip"))
handle_add_ip(argc, argv);
else if (!strcmp(command, "add-node"))
handle_add_node(argc, argv);
else if (!strcmp(command, "remove-ip"))
handle_remove_ip(argc, argv);
else if (!strcmp(command, "remove-node"))
handle_remove_node(argc, argv);
else if (!strcmp(command, "show-ip"))
handle_show_ip(argc, argv);
else if (!strcmp(command, "show-node"))
handle_show_node(argc, argv);
else if (!strcmp(command, "status"))
handle_status(argc, argv);
else
usage();
}
/*
* Read in one track from the file, and return an element tree
* describing it.
*
* Arguments:
* msp - Midi state
*/
static struct trackElement *
read_track(struct midistate *msp)
{
int status, laststatus;
int head;
int length;
int delta_time;
struct trackElement *track;
int i;
laststatus = 0;
head = read_int(msp, 4);
if (head != MIDI_TRACK_MAGIC)
except(formatError,
"Bad track header (%x), probably not a midi file",
head);
length = read_int(msp, 4);
msp->chunk_size = length;
msp->chunk_count = 0; /* nothing read yet */
track = md_track_new();
msp->current_time = 0;
while (msp->chunk_count < msp->chunk_size) {
delta_time = read_var(msp);
msp->current_time += delta_time;
status = read_int(msp, 1);
if ((status & 0x80) == 0) {
/*
* This is not a status byte and so running status is being
* used. Re-use the previous status and push back this byte.
*/
put_back(msp, status);
status = laststatus;
} else {
laststatus = status;
}
handle_status(msp, track, status);
}
restart:
for (i = 0; i < msp->notes->len; i++) {
struct noteElement *ns;
ns = g_ptr_array_index(msp->notes, i);
msp->device = MD_ELEMENT(ns)->device_channel;
printf("Left over note, finishing\n");
finish_note(msp, ns->note, 0);
goto restart;
}
msp->track_count++;
return track;
}
int main(int argc, char** argv) {
int retval = config.parse_file();
if (retval) {
fprintf(stderr, "can't parse config file\n");
exit(1);
}
retval = boinc_db.open(
config.db_name, config.db_host, config.db_user, config.db_passwd
);
if (retval) {
fprintf(stderr, "can't open DB\n");
exit(1);
}
for (int i=1; i<argc; i++) {
if (!strcmp(argv[i], "add")) {
if (argc != 3) usage();
retval = handle_add(argv[++i]);
if (retval) {
fprintf(stderr, "error %d\n", retval);
} else {
printf("file added successfully\n");
}
exit(retval);
}
if (!strcmp(argv[i], "remove")) {
if (argc != 3) usage();
retval = handle_remove(argv[++i]);
if (retval) {
fprintf(stderr, "error %d\n", retval);
} else {
printf("file removed successfully\n");
}
exit(retval);
}
if (!strcmp(argv[i], "retrieve")) {
if (argc != 3) usage();
retval = handle_retrieve(argv[++i]);
if (retval) {
fprintf(stderr, "error %d\n", retval);
} else {
printf("file retrieval started\n");
}
exit(retval);
}
if (!strcmp(argv[i], "status")) {
if (argc != 3) usage();
retval = handle_status(argv[++i]);
if (retval) {
fprintf(stderr, "error %d\n", retval);
}
exit(retval);
}
usage();
}
usage();
}
/* main friend_connections loop. */
void do_friend_connections(Friend_Connections *fr_c, void *userdata)
{
const uint64_t temp_time = mono_time_get(fr_c->mono_time);
for (uint32_t i = 0; i < fr_c->num_cons; ++i) {
Friend_Conn *const friend_con = get_conn(fr_c, i);
if (friend_con) {
if (friend_con->status == FRIENDCONN_STATUS_CONNECTING) {
if (friend_con->dht_pk_lastrecv + FRIEND_DHT_TIMEOUT < temp_time) {
if (friend_con->dht_lock) {
dht_delfriend(fr_c->dht, friend_con->dht_temp_pk, friend_con->dht_lock);
friend_con->dht_lock = 0;
memset(friend_con->dht_temp_pk, 0, CRYPTO_PUBLIC_KEY_SIZE);
}
}
if (friend_con->dht_ip_port_lastrecv + FRIEND_DHT_TIMEOUT < temp_time) {
friend_con->dht_ip_port.ip.family = net_family_unspec;
}
if (friend_con->dht_lock) {
if (friend_new_connection(fr_c, i) == 0) {
set_direct_ip_port(fr_c->net_crypto, friend_con->crypt_connection_id, friend_con->dht_ip_port, 0);
connect_to_saved_tcp_relays(fr_c, i, (MAX_FRIEND_TCP_CONNECTIONS / 2)); /* Only fill it half up. */
}
}
} else if (friend_con->status == FRIENDCONN_STATUS_CONNECTED) {
if (friend_con->ping_lastsent + FRIEND_PING_INTERVAL < temp_time) {
send_ping(fr_c, i);
}
if (friend_con->share_relays_lastsent + SHARE_RELAYS_INTERVAL < temp_time) {
send_relays(fr_c, i);
}
if (friend_con->ping_lastrecv + FRIEND_CONNECTION_TIMEOUT < temp_time) {
/* If we stopped receiving ping packets, kill it. */
crypto_kill(fr_c->net_crypto, friend_con->crypt_connection_id);
friend_con->crypt_connection_id = -1;
handle_status(fr_c, i, 0, userdata); /* Going offline. */
}
}
}
}
if (fr_c->local_discovery_enabled) {
lan_discovery(fr_c);
}
}
/* main friend_connections loop. */
void do_friend_connections(Friend_Connections *fr_c)
{
uint32_t i;
uint64_t temp_time = unix_time();
for (i = 0; i < fr_c->num_cons; ++i) {
Friend_Conn *friend_con = get_conn(fr_c, i);
if (friend_con) {
if (friend_con->status == FRIENDCONN_STATUS_CONNECTING) {
if (friend_con->dht_pk_lastrecv + FRIEND_DHT_TIMEOUT < temp_time) {
if (friend_con->dht_lock) {
DHT_delfriend(fr_c->dht, friend_con->dht_temp_pk, friend_con->dht_lock);
friend_con->dht_lock = 0;
}
}
if (friend_con->dht_ip_port_lastrecv + FRIEND_DHT_TIMEOUT < temp_time) {
friend_con->dht_ip_port.ip.family = 0;
}
if (friend_con->dht_lock) {
if (friend_new_connection(fr_c, i) == 0) {
set_direct_ip_port(fr_c->net_crypto, friend_con->crypt_connection_id, friend_con->dht_ip_port, 0);
connect_to_saved_tcp_relays(fr_c, i, (MAX_FRIEND_TCP_CONNECTIONS / 2)); /* Only fill it half up. */
}
}
} else if (friend_con->status == FRIENDCONN_STATUS_CONNECTED) {
if (friend_con->ping_lastsent + FRIEND_PING_INTERVAL < temp_time) {
send_ping(fr_c, i);
}
if (friend_con->share_relays_lastsent + SHARE_RELAYS_INTERVAL < temp_time) {
send_relays(fr_c, i);
}
if (friend_con->ping_lastrecv + FRIEND_CONNECTION_TIMEOUT < temp_time) {
/* If we stopped receiving ping packets, kill it. */
crypto_kill(fr_c->net_crypto, friend_con->crypt_connection_id);
friend_con->crypt_connection_id = -1;
handle_status(fr_c, i, 0); /* Going offline. */
}
}
}
}
LANdiscovery(fr_c);
}
/**
* Poll for any terminated background processes.
*/
void poll_background_children() {
struct rusage before;
struct rusage after;
int status = 0;
pid_t p;
getrusage(RUSAGE_CHILDREN, &before);
p = waitpid(-1, &status, WNOHANG);
getrusage(RUSAGE_CHILDREN, &after);
if (p > 0) {
printf("Terminated in background: \n");
handle_status(&before, &after, &status);
}
}
/* main friend_connections loop. */
void do_friend_connections(Friend_Connections *fr_c)
{
uint32_t i;
uint64_t temp_time = unix_time();
for (i = 0; i < fr_c->num_cons; ++i) {
Friend_Conn *friend_con = get_conn(fr_c, i);
if (friend_con) {
if (friend_con->status == FRIENDCONN_STATUS_CONNECTING) {
if (friend_con->dht_ping_lastrecv + FRIEND_DHT_TIMEOUT < temp_time) {
if (friend_con->dht_lock) {
DHT_delfriend(fr_c->dht, friend_con->dht_temp_pk, friend_con->dht_lock);
friend_con->dht_lock = 0;
}
}
if (friend_con->dht_ip_port_lastrecv + FRIEND_DHT_TIMEOUT < temp_time) {
friend_con->dht_ip_port.ip.family = 0;
}
if (friend_con->dht_lock) {
if (friend_new_connection(fr_c, i) == 0) {
set_connection_dht_public_key(fr_c->net_crypto, friend_con->crypt_connection_id, friend_con->dht_temp_pk);
set_direct_ip_port(fr_c->net_crypto, friend_con->crypt_connection_id, friend_con->dht_ip_port);
}
}
} else if (friend_con->status == FRIENDCONN_STATUS_CONNECTED) {
if (friend_con->ping_lastsent + FRIEND_PING_INTERVAL < temp_time) {
send_ping(fr_c, i);
}
if (friend_con->ping_lastrecv + FRIEND_CONNECTION_TIMEOUT < temp_time) {
/* If we stopped receiving ping packets, kill it. */
crypto_kill(fr_c->net_crypto, friend_con->crypt_connection_id);
friend_con->crypt_connection_id = -1;
handle_status(fr_c, i, 0); /* Going offline. */
}
}
}
}
}
/**
* Detect terminated processes by signals and handle the exit status.
*/
void clean_up_after_children(int signal_number, siginfo_t *info, void *context) {
int status;
struct rusage before;
struct rusage after;
pid_t p;
if (signal_number == SIGCHLD) {
sighold(SIGCHLD);
getrusage(RUSAGE_CHILDREN, &before);
while ((p = waitpid(-1, &status, WNOHANG)) != -1 && p != 0) {
getrusage(RUSAGE_CHILDREN, &after);
printf("Terminated in background: \n");
handle_status(&before, &after, &status);
getrusage(RUSAGE_CHILDREN, &before);
}
sigrelse(SIGCHLD);
}
}
/** Handle SIGTERM, do not let ourselfs commit suicide. */
static void handle_sigterm(int sig) {
int status;
struct rusage before;
struct rusage after;
if(sig == SIGTERM) {
sighold(SIGTERM);
getrusage(RUSAGE_CHILDREN, &before);
if(waitpid(-1, &status, WNOHANG) == -1) return;
while(waitpid(-1, &status, WNOHANG) == 0) {
getrusage(RUSAGE_CHILDREN, &before);
}
getrusage(RUSAGE_CHILDREN, &after);
printf("Terminated in background by exit in foreground: \n");
handle_status(&before, &after, &status);
sigrelse(SIGTERM);
}
}
/*
* Handle HIL interrupts.
*/
static irqreturn_t hil_interrupt(int irq, void *handle, struct pt_regs *regs)
{
unsigned char s, c;
s = hil_status();
c = hil_read_data();
switch (s >> 4) {
case 0x5:
handle_status(s, c);
break;
case 0x6:
handle_data(s, c);
break;
case 0x4:
hil_dev.s = s;
hil_dev.c = c;
mb();
hil_dev.valid = 1;
break;
}
return IRQ_HANDLED;
}
/*
* This is the serial driver's generic interrupt routine
*/
static void rs_interruptTx(int irq, void *dev_id, struct pt_regs *regs)
{
unsigned int count, status;
struct s3c3410_serial *info = &s3c3410_info;
if (info->x_char) {
outb(info->x_char, S3C3410X_UART_BASE+S3C3410X_UTXH_B);
info->x_char = 0;
return;
}
if ((info->xmit_cnt <= 0) || info->tty->stopped || info->tty->hw_stopped ) {
tx_stop();
return;
}
for (count = TX_FIFO_DEPTH; count > 0; --count) {
status = inb(S3C3410X_UART_BASE+S3C3410X_UFSTAT);
if (status & UFSTAT_TFF) {
handle_status (info, UFSTAT_TFF);
break;
} else {
outb(info->xmit_buf[info->xmit_tail++],
S3C3410X_UART_BASE+S3C3410X_UTXH_B);
info->xmit_tail = info->xmit_tail & (SERIAL_XMIT_SIZE - 1);
if (--info->xmit_cnt <= 0)
break;
}
}
if (info->xmit_cnt < WAKEUP_CHARS)
rs_sched_event(info, RS_EVENT_WRITE_WAKEUP);
if (info->xmit_cnt <= 0)
tx_stop();
return;
}
int sdlog2_thread_main(int argc, char *argv[])
{
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("failed to open MAVLink log stream, start mavlink app first.");
}
/* log buffer size */
int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) {
switch (ch) {
case 'r': {
unsigned long r = strtoul(optarg, NULL, 10);
if (r == 0) {
sleep_delay = 0;
} else {
sleep_delay = 1000000 / r;
}
}
break;
case 'b': {
unsigned long s = strtoul(optarg, NULL, 10);
if (s < 1) {
s = 1;
}
log_buffer_size = 1024 * s;
}
break;
case 'e':
log_on_start = true;
break;
case 'a':
log_when_armed = true;
break;
case '?':
if (optopt == 'c') {
warnx("Option -%c requires an argument.", optopt);
} else if (isprint(optopt)) {
warnx("Unknown option `-%c'.", optopt);
} else {
warnx("Unknown option character `\\x%x'.", optopt);
}
default:
sdlog2_usage("unrecognized flag");
errx(1, "exiting.");
}
}
if (!file_exist(mountpoint)) {
errx(1, "logging mount point %s not present, exiting.", mountpoint);
}
if (create_logfolder()) {
errx(1, "unable to create logging folder, exiting.");
}
const char *converter_in = "/etc/logging/conv.zip";
char *converter_out = malloc(120);
sprintf(converter_out, "%s/conv.zip", folder_path);
if (file_copy(converter_in, converter_out)) {
errx(1, "unable to copy conversion scripts, exiting.");
}
free(converter_out);
/* only print logging path, important to find log file later */
warnx("logging to directory: %s", folder_path);
/* initialize log buffer with specified size */
warnx("log buffer size: %i bytes.", log_buffer_size);
if (OK != logbuffer_init(&lb, log_buffer_size)) {
errx(1, "can't allocate log buffer, exiting.");
}
struct vehicle_status_s buf_status;
memset(&buf_status, 0, sizeof(buf_status));
/* warning! using union here to save memory, elements should be used separately! */
union {
struct vehicle_command_s cmd;
struct sensor_combined_s sensor;
struct vehicle_attitude_s att;
struct vehicle_attitude_setpoint_s att_sp;
struct vehicle_rates_setpoint_s rates_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct actuator_controls_effective_s act_controls_effective;
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
struct vehicle_global_position_setpoint_s global_pos_sp;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
struct airspeed_s airspeed;
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
} buf;
memset(&buf, 0, sizeof(buf));
struct {
int cmd_sub;
int status_sub;
int sensor_sub;
int att_sub;
int att_sp_sub;
int rates_sp_sub;
int act_outputs_sub;
int act_controls_sub;
int act_controls_effective_sub;
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
int global_pos_sp_sub;
int gps_pos_sub;
int vicon_pos_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
int esc_sub;
int global_vel_sp_sub;
} subs;
/* log message buffer: header + body */
#pragma pack(push, 1)
struct {
LOG_PACKET_HEADER;
union {
struct log_TIME_s log_TIME;
struct log_ATT_s log_ATT;
struct log_ATSP_s log_ATSP;
struct log_IMU_s log_IMU;
struct log_SENS_s log_SENS;
struct log_LPOS_s log_LPOS;
struct log_LPSP_s log_LPSP;
struct log_GPS_s log_GPS;
struct log_ATTC_s log_ATTC;
struct log_STAT_s log_STAT;
struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0;
struct log_AIRS_s log_AIRS;
struct log_ARSP_s log_ARSP;
struct log_FLOW_s log_FLOW;
struct log_GPOS_s log_GPOS;
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
};
#pragma pack(pop)
memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 20;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
/* --- VEHICLE COMMAND --- */
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
fds[fdsc_count].fd = subs.cmd_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- VEHICLE STATUS --- */
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
fds[fdsc_count].fd = subs.status_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GPS POSITION --- */
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
fds[fdsc_count].fd = subs.gps_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- SENSORS COMBINED --- */
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
fds[fdsc_count].fd = subs.sensor_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ATTITUDE --- */
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
fds[fdsc_count].fd = subs.att_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ATTITUDE SETPOINT --- */
subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
fds[fdsc_count].fd = subs.att_sp_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- RATES SETPOINT --- */
subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
fds[fdsc_count].fd = subs.rates_sp_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR OUTPUTS --- */
subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
fds[fdsc_count].fd = subs.act_outputs_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR CONTROL --- */
subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
fds[fdsc_count].fd = subs.act_controls_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR CONTROL EFFECTIVE --- */
subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
fds[fdsc_count].fd = subs.act_controls_effective_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- LOCAL POSITION --- */
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
fds[fdsc_count].fd = subs.local_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- LOCAL POSITION SETPOINT --- */
subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
fds[fdsc_count].fd = subs.local_pos_sp_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL POSITION --- */
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
fds[fdsc_count].fd = subs.global_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL POSITION SETPOINT--- */
subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
fds[fdsc_count].fd = subs.global_pos_sp_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- VICON POSITION --- */
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
fds[fdsc_count].fd = subs.vicon_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- OPTICAL FLOW --- */
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
fds[fdsc_count].fd = subs.flow_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- RC CHANNELS --- */
subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
fds[fdsc_count].fd = subs.rc_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- AIRSPEED --- */
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
fds[fdsc_count].fd = subs.airspeed_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ESCs --- */
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
fds[fdsc_count].fd = subs.esc_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL VELOCITY SETPOINT --- */
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
fds[fdsc_count].fd = subs.global_vel_sp_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
*/
if (fdsc_count > fdsc) {
warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__);
fdsc_count = fdsc;
}
/*
* set up poll to block for new data,
* wait for a maximum of 1000 ms
*/
const int poll_timeout = 1000;
thread_running = true;
/* initialize thread synchronization */
pthread_mutex_init(&logbuffer_mutex, NULL);
pthread_cond_init(&logbuffer_cond, NULL);
/* track changes in sensor_combined topic */
uint16_t gyro_counter = 0;
uint16_t accelerometer_counter = 0;
uint16_t magnetometer_counter = 0;
uint16_t baro_counter = 0;
uint16_t differential_pressure_counter = 0;
/* enable logging on start if needed */
if (log_on_start)
sdlog2_start_log();
while (!main_thread_should_exit) {
/* decide use usleep() or blocking poll() */
bool use_sleep = sleep_delay > 0 && logging_enabled;
/* poll all topics if logging enabled or only management (first 2) if not */
int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout);
/* handle the poll result */
if (poll_ret < 0) {
warnx("ERROR: poll error, stop logging.");
main_thread_should_exit = true;
} else if (poll_ret > 0) {
/* check all data subscriptions only if logging enabled,
* logging_enabled can be changed while checking vehicle_command and vehicle_status */
bool check_data = logging_enabled;
int ifds = 0;
int handled_topics = 0;
/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
handle_command(&buf.cmd);
handled_topics++;
}
/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
if (log_when_armed) {
handle_status(&buf_status);
}
handled_topics++;
}
if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
continue;
}
ifds = 1; // begin from fds[1] again
pthread_mutex_lock(&logbuffer_mutex);
/* write time stamp message */
log_msg.msg_type = LOG_TIME_MSG;
log_msg.body.log_TIME.t = hrt_absolute_time();
LOGBUFFER_WRITE_AND_COUNT(TIME);
/* --- VEHICLE STATUS --- */
if (fds[ifds++].revents & POLLIN) {
// Don't orb_copy, it's already done few lines above
log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
LOGBUFFER_WRITE_AND_COUNT(STAT);
}
/* --- GPS POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
log_msg.body.log_GPS.eph = buf.gps_pos.eph_m;
log_msg.body.log_GPS.epv = buf.gps_pos.epv_m;
log_msg.body.log_GPS.lat = buf.gps_pos.lat;
log_msg.body.log_GPS.lon = buf.gps_pos.lon;
log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001f;
log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s;
log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s;
log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s;
log_msg.body.log_GPS.cog = buf.gps_pos.cog_rad;
LOGBUFFER_WRITE_AND_COUNT(GPS);
}
/* --- SENSOR COMBINED --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
bool write_IMU = false;
bool write_SENS = false;
if (buf.sensor.gyro_counter != gyro_counter) {
gyro_counter = buf.sensor.gyro_counter;
write_IMU = true;
}
if (buf.sensor.accelerometer_counter != accelerometer_counter) {
accelerometer_counter = buf.sensor.accelerometer_counter;
write_IMU = true;
}
if (buf.sensor.magnetometer_counter != magnetometer_counter) {
magnetometer_counter = buf.sensor.magnetometer_counter;
write_IMU = true;
}
if (buf.sensor.baro_counter != baro_counter) {
baro_counter = buf.sensor.baro_counter;
write_SENS = true;
}
if (buf.sensor.differential_pressure_counter != differential_pressure_counter) {
differential_pressure_counter = buf.sensor.differential_pressure_counter;
write_SENS = true;
}
if (write_IMU) {
log_msg.msg_type = LOG_IMU_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
if (write_SENS) {
log_msg.msg_type = LOG_SENS_MSG;
log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar;
log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
}
/* --- ATTITUDE --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
log_msg.msg_type = LOG_ATT_MSG;
log_msg.body.log_ATT.roll = buf.att.roll;
log_msg.body.log_ATT.pitch = buf.att.pitch;
log_msg.body.log_ATT.yaw = buf.att.yaw;
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
LOGBUFFER_WRITE_AND_COUNT(ATT);
}
/* --- ATTITUDE SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp);
log_msg.msg_type = LOG_ATSP_MSG;
log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body;
log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body;
log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body;
log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust;
LOGBUFFER_WRITE_AND_COUNT(ATSP);
}
/* --- RATES SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp);
log_msg.msg_type = LOG_ARSP_MSG;
log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll;
log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch;
log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw;
LOGBUFFER_WRITE_AND_COUNT(ARSP);
}
/* --- ACTUATOR OUTPUTS --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs);
log_msg.msg_type = LOG_OUT0_MSG;
memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
LOGBUFFER_WRITE_AND_COUNT(OUT0);
}
/* --- ACTUATOR CONTROL --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls);
log_msg.msg_type = LOG_ATTC_MSG;
log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
log_msg.body.log_ATTC.yaw = buf.act_controls.control[2];
log_msg.body.log_ATTC.thrust = buf.act_controls.control[3];
LOGBUFFER_WRITE_AND_COUNT(ATTC);
}
/* --- ACTUATOR CONTROL EFFECTIVE --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective);
// TODO not implemented yet
}
/* --- LOCAL POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
log_msg.msg_type = LOG_LPOS_MSG;
log_msg.body.log_LPOS.x = buf.local_pos.x;
log_msg.body.log_LPOS.y = buf.local_pos.y;
log_msg.body.log_LPOS.z = buf.local_pos.z;
log_msg.body.log_LPOS.vx = buf.local_pos.vx;
log_msg.body.log_LPOS.vy = buf.local_pos.vy;
log_msg.body.log_LPOS.vz = buf.local_pos.vz;
log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
LOGBUFFER_WRITE_AND_COUNT(LPOS);
}
/* --- LOCAL POSITION SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp);
log_msg.msg_type = LOG_LPSP_MSG;
log_msg.body.log_LPSP.x = buf.local_pos_sp.x;
log_msg.body.log_LPSP.y = buf.local_pos_sp.y;
log_msg.body.log_LPSP.z = buf.local_pos_sp.z;
log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw;
LOGBUFFER_WRITE_AND_COUNT(LPSP);
}
/* --- GLOBAL POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
log_msg.msg_type = LOG_GPOS_MSG;
log_msg.body.log_GPOS.lat = buf.global_pos.lat;
log_msg.body.log_GPOS.lon = buf.global_pos.lon;
log_msg.body.log_GPOS.alt = buf.global_pos.alt;
log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
/* --- GLOBAL POSITION SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
}
/* --- VICON POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
// TODO not implemented yet
}
/* --- FLOW --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
log_msg.msg_type = LOG_FLOW_MSG;
log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
log_msg.body.log_FLOW.quality = buf.flow.quality;
log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
LOGBUFFER_WRITE_AND_COUNT(FLOW);
}
/* --- RC CHANNELS --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(rc_channels), subs.rc_sub, &buf.rc);
log_msg.msg_type = LOG_RC_MSG;
/* Copy only the first 8 channels of 14 */
memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
LOGBUFFER_WRITE_AND_COUNT(RC);
}
/* --- AIRSPEED --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
log_msg.msg_type = LOG_AIRS_MSG;
log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
/* --- ESCs --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
log_msg.msg_type = LOG_ESC_MSG;
log_msg.body.log_ESC.counter = buf.esc.counter;
log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
log_msg.body.log_ESC.esc_num = i;
log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
LOGBUFFER_WRITE_AND_COUNT(ESC);
}
}
/* --- GLOBAL VELOCITY SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp);
log_msg.msg_type = LOG_GVSP_MSG;
log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz;
LOGBUFFER_WRITE_AND_COUNT(GVSP);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
pthread_cond_signal(&logbuffer_cond);
}
/* unlock, now the writer thread may run */
pthread_mutex_unlock(&logbuffer_mutex);
}
if (use_sleep) {
usleep(sleep_delay);
}
}
if (logging_enabled)
sdlog2_stop_log();
pthread_mutex_destroy(&logbuffer_mutex);
pthread_cond_destroy(&logbuffer_cond);
free(lb.data);
warnx("exiting.");
thread_running = false;
return 0;
}
/**
* Check a transfer phase message and pass to appropriate message handler,
* decrypting first if necessary
* Returns 1 on success, 0 on error
*/
int handle_transfer_phase(const unsigned char *packet, unsigned char *decrypted,
const struct sockaddr_in *receiver,
int blocks_this_sec, int section_offset,
int pass, int section, struct finfo_t *finfo)
{
struct uftp_h *header;
const unsigned char *message;
int hostidx;
unsigned int decryptlen, meslen;
uint8_t *func;
struct in_addr srcaddr;
header = (struct uftp_h *)packet;
hostidx = find_client(header->srcaddr);
srcaddr.s_addr = header->srcaddr;
if ((keytype != KEY_NONE) && (header->func == ENCRYPTED)) {
if (hostidx == -1) {
log1(0, 0, "Host %s not in host list", inet_ntoa(srcaddr));
send_abort(finfo, "Not in host list", receiver, &srcaddr, 0, 0);
return 0;
}
if (!validate_and_decrypt(packet, &decrypted, &decryptlen, mtu,
keytype, groupkey, groupsalt, ivlen, hashtype, grouphmackey,
hmaclen, sigtype, destlist[hostidx].encinfo->pubkey,
destlist[hostidx].encinfo->pubkeylen)) {
log1(0, 0, "Rejecting message from %s: decrypt/validate failed",
destlist[hostidx].name);
return 0;
}
func = (uint8_t *)decrypted;
message = decrypted;
meslen = decryptlen;
} else {
if ((keytype != KEY_NONE) && ((header->func == PRSTATUS) ||
(header->func == STATUS) || (header->func == COMPLETE) ||
(header->func == ABORT))) {
log1(0, 0, "Rejecting %s message from %s: not encrypted",
func_name(header->func), inet_ntoa(srcaddr));
return 0;
}
func = (uint8_t *)&header->func;
message = packet + sizeof(struct uftp_h);
meslen = ntohs(header->blsize);
}
if (*func == ABORT) {
handle_abort(message, meslen, hostidx, finfo, &srcaddr);
} else if (hostidx == -1) {
log1(0, 0, "Host %s not in host list", inet_ntoa(srcaddr));
send_abort(finfo, "Not in host list", receiver, &srcaddr, 0, 0);
} else {
switch (destlist[hostidx].status) {
case DEST_ACTIVE:
if (*func == STATUS) {
handle_status(message, meslen, finfo, hostidx,
blocks_this_sec, section_offset, pass, section);
} else if (*func == COMPLETE) {
handle_complete(message, meslen, finfo, hostidx);
} else if ((destlist[hostidx].clientcnt != -1) &&
(*func == PRSTATUS)) {
handle_prstatus(message, meslen, finfo, hostidx,
blocks_this_sec, section_offset, pass, section);
} else {
log1(0, 0, "Received invalid message %s from %s",
func_name(*func), destlist[hostidx].name);
}
break;
case DEST_STATUS:
if (*func == COMPLETE) {
handle_complete(message, meslen, finfo, hostidx);
} else if (*func == STATUS) {
handle_status(message, meslen, finfo, hostidx,
blocks_this_sec, section_offset, pass, section);
} else {
log1(0, 0, "Received invalid message %s from %s",
func_name(*func), destlist[hostidx].name);
}
break;
case DEST_DONE:
if (*func == COMPLETE) {
handle_complete(message, meslen, finfo, hostidx);
} else {
log1(0, 0, "Received invalid message %s from %s",
func_name(*func), destlist[hostidx].name);
}
break;
}
}
return 1;
}
int sdlog2_thread_main(int argc, char *argv[])
{
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("failed to open MAVLink log stream, start mavlink app first");
}
/* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */
useconds_t sleep_delay = 20000;
int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
logging_enabled = false;
/* enable logging on start (-e option) */
bool log_on_start = false;
/* enable logging when armed (-a option) */
bool log_when_armed = false;
log_name_timestamp = false;
flag_system_armed = false;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
/* don't exit from getopt loop to leave getopt global variables in consistent state,
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
switch (ch) {
case 'r': {
unsigned long r = strtoul(optarg, NULL, 10);
if (r == 0) {
r = 1;
}
sleep_delay = 1000000 / r;
}
break;
case 'b': {
unsigned long s = strtoul(optarg, NULL, 10);
if (s < 1) {
s = 1;
}
log_buffer_size = 1024 * s;
}
break;
case 'e':
log_on_start = true;
break;
case 'a':
log_when_armed = true;
break;
case 't':
log_name_timestamp = true;
break;
case '?':
if (optopt == 'c') {
warnx("option -%c requires an argument", optopt);
} else if (isprint(optopt)) {
warnx("unknown option `-%c'", optopt);
} else {
warnx("unknown option character `\\x%x'", optopt);
}
err_flag = true;
break;
default:
warnx("unrecognized flag");
err_flag = true;
break;
}
}
if (err_flag) {
sdlog2_usage(NULL);
}
gps_time = 0;
/* create log root dir */
int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret != 0 && errno != EEXIST) {
err(1, "failed creating log root dir: %s", log_root);
}
/* copy conversion scripts */
const char *converter_in = "/etc/logging/conv.zip";
char *converter_out = malloc(64);
snprintf(converter_out, 64, "%s/conv.zip", log_root);
if (file_copy(converter_in, converter_out) != OK) {
warn("unable to copy conversion scripts");
}
free(converter_out);
/* initialize log buffer with specified size */
warnx("log buffer size: %i bytes", log_buffer_size);
if (OK != logbuffer_init(&lb, log_buffer_size)) {
errx(1, "can't allocate log buffer, exiting");
}
struct vehicle_status_s buf_status;
struct vehicle_gps_position_s buf_gps_pos;
memset(&buf_status, 0, sizeof(buf_status));
memset(&buf_gps_pos, 0, sizeof(buf_gps_pos));
/* warning! using union here to save memory, elements should be used separately! */
union {
struct vehicle_command_s cmd;
struct sensor_combined_s sensor;
struct vehicle_attitude_s att;
struct vehicle_attitude_setpoint_s att_sp;
struct vehicle_rates_setpoint_s rates_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s act_controls;
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
struct position_setpoint_triplet_s triplet;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
struct airspeed_s airspeed;
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
struct battery_status_s battery;
struct telemetry_status_s telemetry;
struct range_finder_report range_finder;
struct estimator_status_report estimator_status;
struct system_power_s system_power;
struct servorail_status_s servorail_status;
} buf;
memset(&buf, 0, sizeof(buf));
/* log message buffer: header + body */
#pragma pack(push, 1)
struct {
LOG_PACKET_HEADER;
union {
struct log_TIME_s log_TIME;
struct log_ATT_s log_ATT;
struct log_ATSP_s log_ATSP;
struct log_IMU_s log_IMU;
struct log_SENS_s log_SENS;
struct log_LPOS_s log_LPOS;
struct log_LPSP_s log_LPSP;
struct log_GPS_s log_GPS;
struct log_ATTC_s log_ATTC;
struct log_STAT_s log_STAT;
struct log_RC_s log_RC;
struct log_OUT0_s log_OUT0;
struct log_AIRS_s log_AIRS;
struct log_ARSP_s log_ARSP;
struct log_FLOW_s log_FLOW;
struct log_GPOS_s log_GPOS;
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
struct log_BATT_s log_BATT;
struct log_DIST_s log_DIST;
struct log_TELE_s log_TELE;
struct log_ESTM_s log_ESTM;
struct log_PWR_s log_PWR;
struct log_VICN_s log_VICN;
struct log_GSN0_s log_GSN0;
struct log_GSN1_s log_GSN1;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
};
#pragma pack(pop)
memset(&log_msg.body, 0, sizeof(log_msg.body));
struct {
int cmd_sub;
int status_sub;
int sensor_sub;
int att_sub;
int att_sp_sub;
int rates_sp_sub;
int act_outputs_sub;
int act_controls_sub;
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
int triplet_sub;
int gps_pos_sub;
int vicon_pos_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
int esc_sub;
int global_vel_sp_sub;
int battery_sub;
int telemetry_sub;
int range_finder_sub;
int estimator_status_sub;
int system_power_sub;
int servorail_status_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
thread_running = true;
/* initialize thread synchronization */
pthread_mutex_init(&logbuffer_mutex, NULL);
pthread_cond_init(&logbuffer_cond, NULL);
/* track changes in sensor_combined topic */
hrt_abstime gyro_timestamp = 0;
hrt_abstime accelerometer_timestamp = 0;
hrt_abstime magnetometer_timestamp = 0;
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
/* enable logging on start if needed */
if (log_on_start) {
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
gps_time = buf_gps_pos.time_gps_usec;
}
}
sdlog2_start_log();
}
while (!main_thread_should_exit) {
usleep(sleep_delay);
/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) {
handle_command(&buf.cmd);
}
/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
if (status_updated) {
if (log_when_armed) {
handle_status(&buf_status);
}
}
/* --- GPS POSITION - LOG MANAGEMENT --- */
bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos);
if (gps_pos_updated && log_name_timestamp) {
gps_time = buf_gps_pos.time_gps_usec;
}
if (!logging_enabled) {
continue;
}
pthread_mutex_lock(&logbuffer_mutex);
/* write time stamp message */
log_msg.msg_type = LOG_TIME_MSG;
log_msg.body.log_TIME.t = hrt_absolute_time();
LOGBUFFER_WRITE_AND_COUNT(TIME);
/* --- VEHICLE STATUS --- */
if (status_updated) {
log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
LOGBUFFER_WRITE_AND_COUNT(STAT);
}
/* --- GPS POSITION --- */
if (gps_pos_updated) {
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
log_msg.body.log_GPS.eph = buf_gps_pos.eph_m;
log_msg.body.log_GPS.epv = buf_gps_pos.epv_m;
log_msg.body.log_GPS.lat = buf_gps_pos.lat;
log_msg.body.log_GPS.lon = buf_gps_pos.lon;
log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f;
log_msg.body.log_GPS.vel_n = buf_gps_pos.vel_n_m_s;
log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
LOGBUFFER_WRITE_AND_COUNT(GPS);
/* log the SNR of each satellite for a detailed view of signal quality */
log_msg.msg_type = LOG_GSN0_MSG;
/* pick the smaller number so we do not overflow any of the arrays */
unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
unsigned log_max_snr = sizeof(log_msg.body.log_GSN0.satellite_snr) / sizeof(log_msg.body.log_GSN0.satellite_snr[0]);
unsigned sat_max_snr = (gps_msg_max_snr < log_max_snr) ? gps_msg_max_snr : log_max_snr;
for (unsigned i = 0; i < sat_max_snr; i++) {
log_msg.body.log_GSN0.satellite_snr[i] = buf_gps_pos.satellite_snr[i];
}
LOGBUFFER_WRITE_AND_COUNT(GSN0);
}
/* --- SENSOR COMBINED --- */
if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
bool write_IMU = false;
bool write_SENS = false;
if (buf.sensor.timestamp != gyro_timestamp) {
gyro_timestamp = buf.sensor.timestamp;
write_IMU = true;
}
if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
write_IMU = true;
}
if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
write_IMU = true;
}
if (buf.sensor.baro_timestamp != barometer_timestamp) {
barometer_timestamp = buf.sensor.baro_timestamp;
write_SENS = true;
}
if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
write_SENS = true;
}
if (write_IMU) {
log_msg.msg_type = LOG_IMU_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
if (write_SENS) {
log_msg.msg_type = LOG_SENS_MSG;
log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar;
log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa;
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
}
/* --- ATTITUDE --- */
if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) {
log_msg.msg_type = LOG_ATT_MSG;
log_msg.body.log_ATT.roll = buf.att.roll;
log_msg.body.log_ATT.pitch = buf.att.pitch;
log_msg.body.log_ATT.yaw = buf.att.yaw;
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
log_msg.body.log_ATT.gx = buf.att.g_comp[0];
log_msg.body.log_ATT.gy = buf.att.g_comp[1];
log_msg.body.log_ATT.gz = buf.att.g_comp[2];
LOGBUFFER_WRITE_AND_COUNT(ATT);
}
/* --- ATTITUDE SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp)) {
log_msg.msg_type = LOG_ATSP_MSG;
log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body;
log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body;
log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body;
log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust;
LOGBUFFER_WRITE_AND_COUNT(ATSP);
}
/* --- RATES SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp)) {
log_msg.msg_type = LOG_ARSP_MSG;
log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll;
log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch;
log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw;
LOGBUFFER_WRITE_AND_COUNT(ARSP);
}
/* --- ACTUATOR OUTPUTS --- */
if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) {
log_msg.msg_type = LOG_OUT0_MSG;
memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
LOGBUFFER_WRITE_AND_COUNT(OUT0);
}
/* --- ACTUATOR CONTROL --- */
if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls)) {
log_msg.msg_type = LOG_ATTC_MSG;
log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
log_msg.body.log_ATTC.yaw = buf.act_controls.control[2];
log_msg.body.log_ATTC.thrust = buf.act_controls.control[3];
LOGBUFFER_WRITE_AND_COUNT(ATTC);
}
/* --- LOCAL POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
log_msg.msg_type = LOG_LPOS_MSG;
log_msg.body.log_LPOS.x = buf.local_pos.x;
log_msg.body.log_LPOS.y = buf.local_pos.y;
log_msg.body.log_LPOS.z = buf.local_pos.z;
log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom;
log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate;
log_msg.body.log_LPOS.vx = buf.local_pos.vx;
log_msg.body.log_LPOS.vy = buf.local_pos.vy;
log_msg.body.log_LPOS.vz = buf.local_pos.vz;
log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7;
log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7;
log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
log_msg.body.log_LPOS.pos_flags = (buf.local_pos.xy_valid ? 1 : 0) |
(buf.local_pos.z_valid ? 2 : 0) |
(buf.local_pos.v_xy_valid ? 4 : 0) |
(buf.local_pos.v_z_valid ? 8 : 0) |
(buf.local_pos.xy_global ? 16 : 0) |
(buf.local_pos.z_global ? 32 : 0);
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
log_msg.body.log_LPOS.eph = buf.local_pos.eph;
log_msg.body.log_LPOS.epv = buf.local_pos.epv;
LOGBUFFER_WRITE_AND_COUNT(LPOS);
}
/* --- LOCAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) {
log_msg.msg_type = LOG_LPSP_MSG;
log_msg.body.log_LPSP.x = buf.local_pos_sp.x;
log_msg.body.log_LPSP.y = buf.local_pos_sp.y;
log_msg.body.log_LPSP.z = buf.local_pos_sp.z;
log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw;
LOGBUFFER_WRITE_AND_COUNT(LPSP);
}
/* --- GLOBAL POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos)) {
log_msg.msg_type = LOG_GPOS_MSG;
log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
log_msg.body.log_GPOS.alt = buf.global_pos.alt;
log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
log_msg.body.log_GPOS.eph = buf.global_pos.eph;
log_msg.body.log_GPOS.epv = buf.global_pos.epv;
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
/* --- GLOBAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
log_msg.body.log_GPSP.type = buf.triplet.current.type;
log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
}
/* --- VICON POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
log_msg.msg_type = LOG_VICN_MSG;
log_msg.body.log_VICN.x = buf.vicon_pos.x;
log_msg.body.log_VICN.y = buf.vicon_pos.y;
log_msg.body.log_VICN.z = buf.vicon_pos.z;
log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch;
log_msg.body.log_VICN.roll = buf.vicon_pos.roll;
log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
LOGBUFFER_WRITE_AND_COUNT(VICN);
}
/* --- FLOW --- */
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
log_msg.msg_type = LOG_FLOW_MSG;
log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
log_msg.body.log_FLOW.quality = buf.flow.quality;
log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
LOGBUFFER_WRITE_AND_COUNT(FLOW);
}
/* --- RC CHANNELS --- */
if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) {
log_msg.msg_type = LOG_RC_MSG;
/* Copy only the first 8 channels of 14 */
memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
log_msg.body.log_RC.channel_count = buf.rc.chan_count;
log_msg.body.log_RC.signal_lost = buf.rc.signal_lost;
LOGBUFFER_WRITE_AND_COUNT(RC);
}
/* --- AIRSPEED --- */
if (copy_if_updated(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed)) {
log_msg.msg_type = LOG_AIRS_MSG;
log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
/* --- ESCs --- */
if (copy_if_updated(ORB_ID(esc_status), subs.esc_sub, &buf.esc)) {
for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
log_msg.msg_type = LOG_ESC_MSG;
log_msg.body.log_ESC.counter = buf.esc.counter;
log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
log_msg.body.log_ESC.esc_num = i;
log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
LOGBUFFER_WRITE_AND_COUNT(ESC);
}
}
/* --- GLOBAL VELOCITY SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp)) {
log_msg.msg_type = LOG_GVSP_MSG;
log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz;
LOGBUFFER_WRITE_AND_COUNT(GVSP);
}
/* --- BATTERY --- */
if (copy_if_updated(ORB_ID(battery_status), subs.battery_sub, &buf.battery)) {
log_msg.msg_type = LOG_BATT_MSG;
log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
log_msg.body.log_BATT.current = buf.battery.current_a;
log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* --- SYSTEM POWER RAILS --- */
if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
log_msg.msg_type = LOG_PWR_MSG;
log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
/* copy servo rail status topic here too */
orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
LOGBUFFER_WRITE_AND_COUNT(PWR);
}
/* --- TELEMETRY --- */
if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
log_msg.msg_type = LOG_TELE_MSG;
log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
log_msg.body.log_TELE.noise = buf.telemetry.noise;
log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
LOGBUFFER_WRITE_AND_COUNT(TELE);
}
/* --- BOTTOM DISTANCE --- */
if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) {
log_msg.msg_type = LOG_DIST_MSG;
log_msg.body.log_DIST.bottom = buf.range_finder.distance;
log_msg.body.log_DIST.bottom_rate = 0.0f;
log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0);
LOGBUFFER_WRITE_AND_COUNT(DIST);
}
/* --- ESTIMATOR STATUS --- */
if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) {
log_msg.msg_type = LOG_ESTM_MSG;
unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s);
memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s));
memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy);
log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states;
log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan;
log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan;
log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan;
LOGBUFFER_WRITE_AND_COUNT(ESTM);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
pthread_cond_signal(&logbuffer_cond);
}
/* unlock, now the writer thread may run */
pthread_mutex_unlock(&logbuffer_mutex);
}
if (logging_enabled) {
sdlog2_stop_log();
}
pthread_mutex_destroy(&logbuffer_mutex);
pthread_cond_destroy(&logbuffer_cond);
free(lb.data);
warnx("exiting");
thread_running = false;
return 0;
}
/**
* This is the main message reading loop. Messages are read, validated,
* decrypted if necessary, then passed to the appropriate routine for handling.
*/
void mainloop()
{
struct uftp_h *header;
unsigned char *buf, *decrypted, *message;
int packetlen, listidx, hostidx, i;
unsigned int decryptlen, meslen;
uint8_t *func;
struct sockaddr_in src;
struct in_addr srcaddr;
struct timeval *tv;
const int bsize = 9000; // Roughly size of ethernet jumbo frame
log0(0, 0, "%s", VERSIONSTR);
for (i = 0; i < key_count; i++) {
log(0, 0, "Loaded key with fingerprint %s",
print_key_fingerprint(privkey[i]));
}
buf = calloc(bsize, 1);
decrypted = calloc(bsize, 1);
if ((buf == NULL) || (decrypted == NULL)) {
syserror(0, 0, "calloc failed!");
exit(1);
}
header = (struct uftp_h *)buf;
while (1) {
tv = getrecenttimeout();
if (read_packet(listener, &src, buf, &packetlen,
bsize, tv) <= 0) {
continue;
}
if ((header->uftp_id != UFTP_VER_NUM) &&
(header->uftp_id != UFTP_3_0_VER)) {
log(0, 0, "Invalid message from %s: not uftp packet "
"or invalid version", inet_ntoa(src.sin_addr));
continue;
}
if (packetlen != sizeof(struct uftp_h) + ntohs(header->blsize)) {
log(0, 0, "Invalid packet size from %s: got %d, expected %d",
inet_ntoa(src.sin_addr), packetlen,
sizeof(struct uftp_h) + ntohs(header->blsize));
continue;
}
if ((src.sin_addr.s_addr == out_addr.s_addr) &&
(src.sin_port == htons(port))) {
// Packet from self -- drop
continue;
}
if (header->func == HB_REQ) {
handle_hb_request(&src, buf);
continue;
}
if (header->func == HB_RESP) {
handle_hb_response(listener, &src, buf, hb_hosts, hbhost_count,
noname, privkey[0]);
continue;
}
if (header->func == KEY_REQ) {
handle_key_req(&src, buf);
continue;
}
if (header->func == PROXY_KEY) {
// Only clients handle these, so drop
continue;
}
if ((proxy_type == SERVER_PROXY) &&
(down_addr.sin_addr.s_addr == INADDR_ANY)) {
log(0, 0, "Rejecting message from %s: downstream address "
"not established", inet_ntoa(src.sin_addr));
continue;
}
listidx = find_group(ntohl(header->group_id));
if (header->func == ANNOUNCE) {
handle_announce(listidx, &src, buf);
} else {
if (listidx == -1) {
continue;
}
if (proxy_type == SERVER_PROXY) {
// Server proxies don't do anything outside of an ANNOUNCE.
// Just send it on through.
forward_message(listidx, &src, buf);
continue;
}
if (header->func == ABORT) {
handle_abort(listidx, &src, buf);
continue;
}
if (!memcmp(&src, &group_list[listidx].up_addr, sizeof(src))) {
// Downstream message
if (header->func == KEYINFO) {
handle_keyinfo(listidx, buf);
} else if ((header->func == REG_CONF) &&
(group_list[listidx].keytype != KEY_NONE)) {
handle_regconf(listidx, buf);
} else {
// If we don't need to process the message, don't bother
// decrypting anything. Just forward it on.
forward_message(listidx, &src, buf);
}
} else {
// Upstream message
// Decrypt first if necessary
hostidx = find_client(listidx, header->srcaddr);
if ((hostidx != -1) && (header->func == ENCRYPTED) &&
(group_list[listidx].keytype != KEY_NONE)) {
if (!validate_and_decrypt(buf, &decrypted, &decryptlen,
group_list[listidx].mtu,group_list[listidx].keytype,
group_list[listidx].groupkey,
group_list[listidx].groupsalt,
group_list[listidx].ivlen,
group_list[listidx].hashtype,
group_list[listidx].grouphmackey,
group_list[listidx].hmaclen,
group_list[listidx].sigtype,
group_list[listidx].destinfo[hostidx].pubkey,
group_list[listidx].destinfo[hostidx].pubkeylen)) {
log(ntohl(header->group_id), 0, "Rejecting message "
"from %s: decrypt/validate failed",
inet_ntoa(src.sin_addr));
continue;
}
func = (uint8_t *)decrypted;
message = decrypted;
meslen = decryptlen;
} else {
if ((hostidx != -1) &&
(group_list[listidx].keytype != KEY_NONE) &&
((header->func == INFO_ACK) ||
(header->func == STATUS) ||
(header->func == COMPLETE))) {
log(ntohl(header->group_id), 0, "Rejecting %s message "
"from %s: not encrypted",
func_name(header->func),
inet_ntoa(src.sin_addr));
continue;
}
func = (uint8_t *)&header->func;
message = buf + sizeof(struct uftp_h);
meslen = ntohs(header->blsize);
}
if ((hostidx == -1) && (header->srcaddr == 0)) {
srcaddr = src.sin_addr;
} else {
srcaddr.s_addr = header->srcaddr;
}
switch (*func) {
case REGISTER:
handle_register(listidx, hostidx, message, meslen,
srcaddr.s_addr);
break;
case CLIENT_KEY:
handle_clientkey(listidx, hostidx, message, meslen,
srcaddr.s_addr);
break;
case INFO_ACK:
handle_info_ack(listidx, hostidx, message, meslen);
break;
case STATUS:
handle_status(listidx, hostidx, message, meslen);
break;
case COMPLETE:
handle_complete(listidx, hostidx, message, meslen);
break;
default:
forward_message(listidx, &src, buf);
break;
}
}
}
}
}