int main(void) {
/* generic hw init */
hw_init();
/* init uart */
uart_init();
#ifdef DEBUG_INIT
printf("AlceOSD hw%dv%d fw%d.%d.%d\r\n", hw_rev >> 4, hw_rev & 0xf, VERSION_MAJOR, VERSION_MINOR, VERSION_DEV);
if (RCONbits.WDTO)
printf("watchdog reset\r\n");
if (RCONbits.EXTR)
printf("external reset\r\n");
if (RCONbits.SWR)
printf("software reset\r\n");
if (RCONbits.IOPUWR)
printf("ill opcode / uninit W reset\r\n");
if (RCONbits.WDTO)
printf("trap conflict reset\r\n");
#endif
/* real time clock init */
clock_init();
/* adc init */
adc_init();
/* init video driver */
init_video();
/* try to load config from flash */
config_init();
/* init widget modules */
widgets_init();
/* setup tabs */
tabs_init();
/* welcome message */
console_printf("AlceOSD hw%dv%d fw%d.%d.%d\n", hw_rev >> 4, hw_rev & 0xf, VERSION_MAJOR, VERSION_MINOR, VERSION_DEV);
/* init home tracking */
init_home();
/* init flight status tracking */
init_flight_stats();
/* init mavlink module */
mavlink_init();
/* init uavtalk module */
uavtalk_init();
/* link serial ports to processes */
uart_set_config_clients(1);
/* enable all interrupts */
_IPL = 0;
_IPL3 = 1;
console_printf("Processes running...\n");
/* main loop */
process_run();
return 0;
}
int main(int argc, char *argv[]) {
char szErrbuf[PCAP_ERRBUF_SIZE];
int i;
pcap_t *ppcap = NULL;
char fBrokenSocket = 0;
int pcnt = 0;
time_t start_time;
uint8_t packet_transmit_buffer[MAX_PACKET_LENGTH];
size_t packet_header_length = 0;
fd_set fifo_set;
int max_fifo_fd = -1;
fifo_t fifo[MAX_FIFOS];
int param_transmission_count = 1;
int param_data_packets_per_block = 8;
int param_fec_packets_per_block = 4;
int param_packet_length = MAX_USER_PACKET_LENGTH;
int param_port = 0;
int param_min_packet_length = 0;
int param_fifo_count = 1;
strcpy(param_serial_port, "/dev/ttyUSB0");
running = 1;
printf("Raw data transmitter (c) 2015 befinitiv GPL2\n");
while (1) {
int nOptionIndex;
static const struct option optiona[] = {
{ "help", no_argument, &flagHelp, 1 },
{ 0, 0, 0, 0 }
};
int c = getopt_long(argc, argv, "r:hf:p:b:m:s:x:P:B:", optiona, &nOptionIndex);
if (c == -1) {
break;
}
switch (c) {
case 0: // long option
break;
case 'h': // help
usage();
case 'r': // retransmissions
param_fec_packets_per_block = atoi(optarg);
break;
case 'f': // MTU
param_packet_length = atoi(optarg);
break;
case 'p': //port
param_port = atoi(optarg);
break;
case 'b': //retransmission block size
param_data_packets_per_block = atoi(optarg);
break;
case 'm'://minimum packet length
param_min_packet_length = atoi(optarg);
break;
case 's': //how many streams (fifos) do we have in parallel
param_fifo_count = atoi(optarg);
break;
case 'x': //how often is a block transmitted
param_transmission_count = atoi(optarg);
break;
case 'P': //Serial-Port
strcpy(param_serial_port, optarg);
break;
case 'B': //Serial-Baud
param_serial_baud = atoi(optarg);
break;
default:
printf("unknown switch %c\n", c);
usage();
break;
}
}
if (optind >= argc) {
usage();
}
mavlink_init(0, param_serial_port, param_serial_baud);
#ifdef SDL2
wifibc_thread_telemetry = SDL_CreateThread(wifibc_update_telemetry, NULL, NULL);
#else
wifibc_thread_telemetry = SDL_CreateThread(wifibc_update_telemetry, NULL);
#endif
if (param_packet_length > MAX_USER_PACKET_LENGTH) {
printf("Packet length is limited to %d bytes (you requested %d bytes)\n", MAX_USER_PACKET_LENGTH, param_packet_length);
return (1);
}
if (param_min_packet_length > param_packet_length) {
printf("Your minimum packet length is higher that your maximum packet length (%d > %d)\n", param_min_packet_length, param_packet_length);
return (1);
}
if (param_fifo_count > MAX_FIFOS) {
printf("The maximum number of streams (FIFOS) is %d (you requested %d)\n", MAX_FIFOS, param_fifo_count);
return (1);
}
if (param_data_packets_per_block > MAX_DATA_OR_FEC_PACKETS_PER_BLOCK || param_fec_packets_per_block > MAX_DATA_OR_FEC_PACKETS_PER_BLOCK) {
printf("Data and FEC packets per block are limited to %d (you requested %d data, %d FEC)\n", MAX_DATA_OR_FEC_PACKETS_PER_BLOCK,
param_data_packets_per_block, param_fec_packets_per_block);
return (1);
}
packet_header_length = packet_header_init(packet_transmit_buffer);
fifo_init(fifo, param_fifo_count, param_data_packets_per_block);
fifo_open(fifo, param_fifo_count);
fifo_create_select_set(fifo, param_fifo_count, &fifo_set, &max_fifo_fd);
//initialize forward error correction
fec_init();
// open the interface in pcap
szErrbuf[0] = '\0';
ppcap = pcap_open_live(argv[optind], 800, 1, 20, szErrbuf);
if (ppcap == NULL) {
printf("Unable to open interface %s in pcap: %s\n",
argv[optind], szErrbuf);
return (1);
}
pcap_setnonblock(ppcap, 1, szErrbuf);
start_time = time(NULL);
while (!fBrokenSocket) {
fd_set rdfs;
int ret;
rdfs = fifo_set;
//wait for new data on the fifos
ret = select(max_fifo_fd + 1, &rdfs, NULL, NULL, NULL);
if (ret < 0) {
perror("select");
return (1);
}
//cycle through all fifos and look for new data
for (i = 0; i < param_fifo_count && ret; ++i) {
if (!FD_ISSET(fifo[i].fd, &rdfs)) {
continue;
}
ret--;
packet_buffer_t *pb = fifo[i].pbl + fifo[i].curr_pb;
//if the buffer is fresh we add a payload header
if (pb->len == 0) {
pb->len += sizeof(payload_header_t); //make space for a length field (will be filled later)
}
//read the data
int inl = read(fifo[i].fd, pb->data + pb->len, param_packet_length - pb->len - sizeof(ModelDataMinimal));
if (inl < 0 || inl > param_packet_length - pb->len - sizeof(ModelDataMinimal)) {
perror("reading stdin");
return 1;
}
if (inl == 0) {
//EOF
printf("Warning: Lost connection to fifo %d. Please make sure that a data source is connected\n", i);
usleep(1e5);
continue;
}
pb->len += inl;
// add telemtry-data
ModelDataMinimal.p_lat = ModelData[0].p_lat;
ModelDataMinimal.p_long = ModelData[0].p_long;
ModelDataMinimal.p_alt = ModelData[0].p_alt;
ModelDataMinimal.pitch = ModelData[0].pitch;
ModelDataMinimal.roll = ModelData[0].roll;
ModelDataMinimal.yaw = ModelData[0].yaw;
ModelDataMinimal.speed = ModelData[0].speed;
ModelDataMinimal.voltage = ModelData[0].voltage;
ModelDataMinimal.ampere = ModelData[0].ampere;
ModelDataMinimal.gpsfix = ModelData[0].gpsfix;
ModelDataMinimal.numSat = ModelData[0].numSat;
memcpy(pb->data + pb->len + param_packet_length, &ModelDataMinimal, sizeof(ModelDataMinimal));
pb->len += sizeof(ModelDataMinimal);
//check if this packet is finished
if (pb->len >= param_min_packet_length) {
payload_header_t *ph = (payload_header_t *)pb->data;
ph->data_length = pb->len - sizeof(
payload_header_t); //write the length into the packet. this is needed since with fec we cannot use the wifi packet lentgh anymore. We could also set the user payload to a fixed size but this would introduce additional latency since tx would need to wait until that amount of data has been received
pcnt++;
//check if this block is finished
if (fifo[i].curr_pb == param_data_packets_per_block - 1) {
pb_transmit_block(fifo[i].pbl, ppcap, &(fifo[i].seq_nr), i + param_port, param_packet_length, packet_transmit_buffer, packet_header_length,
param_data_packets_per_block, param_fec_packets_per_block, param_transmission_count);
fifo[i].curr_pb = 0;
} else {
fifo[i].curr_pb++;
}
}
}
if (pcnt % 64 == 0) {
printf("%d data packets sent (interface rate: %.3f)\n", pcnt, 1.0 * pcnt / param_data_packets_per_block * (param_data_packets_per_block + param_fec_packets_per_block) / (time(NULL) - start_time));
}
}
running = 0;
SDL_WaitThread(wifibc_thread_telemetry, NULL);
mavlink_exit(0);
printf("Broken socket\n");
return (0);
}
