int main(int argc, char** argv) {
if (argc != 2) {
puts("Usage: sendpkt <iface>");
exit(1);
}
int sock = NEWSOCKET();
if (sock < 0) {
perror("socket");
exit(1);
}
int sz = read_upto(0, pkt, sizeof(pkt));
if(sz < 0) {
perror("read");
exit(1);
}
struct sockaddr sa;
strncpy(sa.sa_data, argv[1], sizeof(sa.sa_data)); /* device to send it over */
int err = sendto(sock, pkt, sz, 0, &sa, sizeof(sa));
if (err < 0) {
perror("sendto");
exit(1);
}
exit(0);
}
// read a number or a symbol, leave it on the stack
void read_atom(Process & proc, std::istream & in) {
std::string res = read_upto(in);
std::stringstream s(res);
if (res.find('.')!=-1) { // try to parse a float
double f;
s >> f;
if (!s) // it's a symbol
proc.stack.push(Object::to_ref(symbols->lookup(res)));
else
proc.stack.push(Object::to_ref(Float::mk(proc, f)));
}
int libambit_pmem20_log_next_header(ambit_object_t *object, ambit_log_header_t *log_header)
{
int ret = -1;
size_t buffer_offset;
uint16_t tmp_len;
LOG_INFO("Reading header of next log entry");
if (!object->pmem20.log.initialized) {
LOG_ERROR("Trying to get next log without initialization");
return -1;
}
// Check if we reached end of entries
if (object->pmem20.log.current.current == object->pmem20.log.current.next) {
LOG_INFO("No more entries to read");
return 0;
}
if (read_upto(object, object->pmem20.log.current.next, PMEM20_LOG_HEADER_MIN_LEN) == 0) {
buffer_offset = (object->pmem20.log.current.next - PMEM20_LOG_START);
// First check that header seems to be correctly present
if (strncmp((char*)object->pmem20.log.buffer + buffer_offset, "PMEM", 4) == 0) {
object->pmem20.log.current.current = object->pmem20.log.current.next;
buffer_offset += 4;
object->pmem20.log.current.next = read32inc(object->pmem20.log.buffer, &buffer_offset);
object->pmem20.log.current.prev = read32inc(object->pmem20.log.buffer, &buffer_offset);
tmp_len = read16inc(object->pmem20.log.buffer, &buffer_offset);
buffer_offset += tmp_len;
tmp_len = read16inc(object->pmem20.log.buffer, &buffer_offset);
if (libambit_pmem20_log_parse_header(object->pmem20.log.buffer + buffer_offset, tmp_len, log_header) == 0) {
LOG_INFO("Log entry header parsed");
ret = 1;
}
else {
LOG_ERROR("Failed to parse log entry header correctly");
}
}
else {
LOG_ERROR("Failed to find valid log entry header start");
}
}
else {
LOG_WARNING("Failed to read log entry header");
}
// Unset initialized of something went wrong
if (ret < 0) {
object->pmem20.log.initialized = false;
}
return ret;
}
ambit_log_entry_t *libambit_pmem20_log_read_entry(ambit_object_t *object)
{
// Note! We assume that the caller has called libambit_pmem20_log_next_header just before
uint8_t *periodic_sample_spec;
uint16_t tmp_len, sample_len;
size_t buffer_offset, sample_count = 0, i;
ambit_log_entry_t *log_entry;
ambit_log_sample_t *last_periodic = NULL, *utcsource = NULL, *altisource = NULL;
ambit_date_time_t utcbase;
uint32_t altisource_index = 0;
uint32_t last_base_lat = 0, last_base_long = 0;
uint32_t last_small_lat = 0, last_small_long = 0;
uint32_t last_ehpe = 0;
if (!object->pmem20.log.initialized) {
LOG_ERROR("Trying to get log entry without initialization");
return NULL;
}
// Allocate log entry
if ((log_entry = calloc(1, sizeof(ambit_log_entry_t))) == NULL) {
object->pmem20.log.initialized = false;
return NULL;
}
LOG_INFO("Reading log entry from address=%08x", object->pmem20.log.current.current);
buffer_offset = (object->pmem20.log.current.current - PMEM20_LOG_START);
buffer_offset += 12;
// Read samples content definition
tmp_len = read16inc(object->pmem20.log.buffer, &buffer_offset);
periodic_sample_spec = object->pmem20.log.buffer + buffer_offset;
buffer_offset += tmp_len;
// Parse header
tmp_len = read16inc(object->pmem20.log.buffer, &buffer_offset);
if (libambit_pmem20_log_parse_header(object->pmem20.log.buffer + buffer_offset, tmp_len, &log_entry->header) != 0) {
LOG_ERROR("Failed to parse log entry header correctly");
free(log_entry);
object->pmem20.log.initialized = false;
return NULL;
}
buffer_offset += tmp_len;
// Now that we know number of samples, allocate space for them!
if ((log_entry->samples = calloc(log_entry->header.samples_count, sizeof(ambit_log_sample_t))) == NULL) {
free(log_entry);
object->pmem20.log.initialized = false;
return NULL;
}
log_entry->samples_count = log_entry->header.samples_count;
LOG_INFO("Log entry got %d samples, reading", log_entry->samples_count);
// OK, so we are at start of samples, get them all!
while (sample_count < log_entry->samples_count) {
/* NOTE! The double reads below seems a bit unoptimized,
but if we need optimization, we should optimize read_upto
instead...
To ease the pain on wraparound we simply duplicate the sample
to the end of the buffer. */
// First check for log area wrap
if (buffer_offset >= PMEM20_LOG_SIZE - 1) {
read_upto(object, PMEM20_LOG_START + PMEM20_LOG_WRAP_START_OFFSET, 2);
sample_len = read16(object->pmem20.log.buffer, PMEM20_LOG_WRAP_START_OFFSET);
}
else if (buffer_offset == PMEM20_LOG_SIZE - 2) {
read_upto(object, PMEM20_LOG_START + PMEM20_LOG_WRAP_START_OFFSET, 1);
sample_len = object->pmem20.log.buffer[buffer_offset] | (object->pmem20.log.buffer[PMEM20_LOG_WRAP_START_OFFSET] << 8);
}
else {
read_upto(object, PMEM20_LOG_START + buffer_offset, 2);
sample_len = read16(object->pmem20.log.buffer, buffer_offset);
}
// Read all data
if (buffer_offset + 2 < (PMEM20_LOG_SIZE-1)) {
read_upto(object, PMEM20_LOG_START + buffer_offset + 2, sample_len);
}
if (buffer_offset + 2 + sample_len > PMEM20_LOG_SIZE) {
read_upto(object, PMEM20_LOG_START + PMEM20_LOG_WRAP_START_OFFSET, (buffer_offset + 2 + sample_len) - PMEM20_LOG_SIZE);
memcpy(object->pmem20.log.buffer + PMEM20_LOG_SIZE, object->pmem20.log.buffer + PMEM20_LOG_WRAP_START_OFFSET, (buffer_offset + 2 + sample_len) - PMEM20_LOG_SIZE);
}
if (parse_sample(object->pmem20.log.buffer, buffer_offset, &periodic_sample_spec, log_entry, &sample_count) == 1) {
// Calculate times
if (log_entry->samples[sample_count-1].type == ambit_log_sample_type_periodic) {
last_periodic = &log_entry->samples[sample_count-1];
}
else if (last_periodic != NULL) {
log_entry->samples[sample_count-1].time += last_periodic->time;
}
else {
log_entry->samples[sample_count-1].time = 0;
}
if (utcsource == NULL && log_entry->samples[sample_count-1].type == ambit_log_sample_type_gps_base) {
utcsource = &log_entry->samples[sample_count-1];
// Calculate UTC base time
add_time(&utcsource->u.gps_base.utc_base_time, 0-utcsource->time, &utcbase);
}
// Calculate positions
if (log_entry->samples[sample_count-1].type == ambit_log_sample_type_gps_base) {
last_base_lat = log_entry->samples[sample_count-1].u.gps_base.latitude;
last_base_long = log_entry->samples[sample_count-1].u.gps_base.longitude;
last_small_lat = log_entry->samples[sample_count-1].u.gps_base.latitude;
last_small_long = log_entry->samples[sample_count-1].u.gps_base.longitude;
last_ehpe = log_entry->samples[sample_count-1].u.gps_base.ehpe;
}
else if (log_entry->samples[sample_count-1].type == ambit_log_sample_type_gps_small) {
log_entry->samples[sample_count-1].u.gps_small.latitude = last_base_lat + log_entry->samples[sample_count-1].u.gps_small.latitude*10;
log_entry->samples[sample_count-1].u.gps_small.longitude = last_base_long + log_entry->samples[sample_count-1].u.gps_small.longitude*10;
last_small_lat = log_entry->samples[sample_count-1].u.gps_small.latitude;
last_small_long = log_entry->samples[sample_count-1].u.gps_small.longitude;
last_ehpe = log_entry->samples[sample_count-1].u.gps_small.ehpe;
}
else if (log_entry->samples[sample_count-1].type == ambit_log_sample_type_gps_tiny) {
log_entry->samples[sample_count-1].u.gps_tiny.latitude = last_small_lat + log_entry->samples[sample_count-1].u.gps_tiny.latitude*10;
log_entry->samples[sample_count-1].u.gps_tiny.longitude = last_small_long + log_entry->samples[sample_count-1].u.gps_tiny.longitude*10;
log_entry->samples[sample_count-1].u.gps_tiny.ehpe = (last_ehpe > 700 ? 700 : last_ehpe);
last_small_lat = log_entry->samples[sample_count-1].u.gps_tiny.latitude;
last_small_long = log_entry->samples[sample_count-1].u.gps_tiny.longitude;
}
if (altisource == NULL && log_entry->samples[sample_count-1].type == ambit_log_sample_type_altitude_source) {
altisource = &log_entry->samples[sample_count-1];
altisource_index = sample_count-1;
}
}
buffer_offset += 2 + sample_len;
// Wrap
if (buffer_offset >= PMEM20_LOG_SIZE) {
buffer_offset = PMEM20_LOG_WRAP_START_OFFSET + (buffer_offset - PMEM20_LOG_SIZE);
}
}
// Loop through samples again and correct times etc
for (sample_count = 0; sample_count < log_entry->header.samples_count; sample_count++) {
// Set UTC times (if UTC source found)
if (utcsource != NULL) {
add_time(&utcbase, log_entry->samples[sample_count].time, &log_entry->samples[sample_count].utc_time);
}
// Correct altitude based on altitude offset in altitude source
if (altisource != NULL && log_entry->samples[sample_count].type == ambit_log_sample_type_periodic && sample_count < altisource_index) {
for (i=0; i<log_entry->samples[sample_count].u.periodic.value_count; i++) {
if (log_entry->samples[sample_count].u.periodic.values[i].type == ambit_log_sample_periodic_type_sealevelpressure) {
log_entry->samples[sample_count].u.periodic.values[i].u.sealevelpressure += altisource->u.altitude_source.pressure_offset;
}
if (log_entry->samples[sample_count].u.periodic.values[i].type == ambit_log_sample_periodic_type_altitude) {
log_entry->samples[sample_count].u.periodic.values[i].u.altitude += altisource->u.altitude_source.altitude_offset;
}
}
}
}
return log_entry;
}
