void loop() { static uint32_t last_msg_ms; // Update GPS state based on possible bytes received from the module. gps.update(); // If new GPS data is received, output it's contents to the console // Here we rely on the time of the message in GPS class and the time of last message // saved in static variable last_msg_ms. When new message is received, the time // in GPS class will be updated. if (last_msg_ms != gps.last_message_time_ms()) { // Reset the time of message last_msg_ms = gps.last_message_time_ms(); // Acquire location const Location &loc = gps.location(); // Print the contents of message hal.console->print("Lat: "); print_latlon(hal.console, loc.lat); hal.console->print(" Lon: "); print_latlon(hal.console, loc.lng); hal.console->printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %u/%lu STATUS: %u\n", loc.alt * 0.01f, gps.ground_speed(), (int)gps.ground_course_cd() / 100, gps.num_sats(), gps.time_week(), (unsigned long)gps.time_week_ms(), gps.status()); } else { hal.console->println("It seemed like NO GPS"); } // Delay for 10 mS will give us 100 Hz invocation rate hal.scheduler->delay(10); }
/* read and print a log entry using the format strings from the given structure */ void DataFlash_Class::_print_log_entry(uint8_t msg_type, print_mode_fn print_mode, AP_HAL::BetterStream *port) { uint8_t i; for (i=0; i<_num_types; i++) { if (msg_type == PGM_UINT8(&_structures[i].msg_type)) { break; } } if (i == _num_types) { port->printf_P(PSTR("UNKN, %u\n"), (unsigned)msg_type); return; } uint8_t msg_len = PGM_UINT8(&_structures[i].msg_len) - 3; uint8_t pkt[msg_len]; ReadBlock(pkt, msg_len); port->printf_P(PSTR("%S, "), _structures[i].name); for (uint8_t ofs=0, fmt_ofs=0; ofs<msg_len; fmt_ofs++) { char fmt = PGM_UINT8(&_structures[i].format[fmt_ofs]); switch (fmt) { case 'b': { port->printf_P(PSTR("%d"), (int)pkt[ofs]); ofs += 1; break; } case 'B': { port->printf_P(PSTR("%u"), (unsigned)pkt[ofs]); ofs += 1; break; } case 'h': { int16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%d"), (int)v); ofs += sizeof(v); break; } case 'H': { uint16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%u"), (unsigned)v); ofs += sizeof(v); break; } case 'i': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%ld"), (long)v); ofs += sizeof(v); break; } case 'I': { uint32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%lu"), (unsigned long)v); ofs += sizeof(v); break; } case 'f': { float v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%f"), (double)v); ofs += sizeof(v); break; } case 'c': { int16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'C': { uint16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'e': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'E': { uint32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'L': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); print_latlon(port, v); ofs += sizeof(v); break; } case 'n': { char v[5]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'N': { char v[17]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'Z': { char v[65]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'M': { print_mode(port, pkt[ofs]); ofs += 1; break; } default: ofs = msg_len; break; } if (ofs < msg_len) { port->printf_P(PSTR(", ")); } } port->println(); }
/* read and print a log entry using the format strings from the given structure - this really should in in the frontend, not the backend */ void DataFlash_Backend::_print_log_entry(uint8_t msg_type, print_mode_fn print_mode, AP_HAL::BetterStream *port) { uint8_t i; for (i=0; i<_num_types; i++) { if (msg_type == PGM_UINT8(&_structures[i].msg_type)) { break; } } if (i == _num_types) { port->printf_P(PSTR("UNKN, %u\n"), (unsigned)msg_type); return; } uint8_t msg_len = PGM_UINT8(&_structures[i].msg_len) - 3; uint8_t pkt[msg_len]; if (!ReadBlock(pkt, msg_len)) { return; } port->printf_P(PSTR("%S, "), _structures[i].name); for (uint8_t ofs=0, fmt_ofs=0; ofs<msg_len; fmt_ofs++) { char fmt = PGM_UINT8(&_structures[i].format[fmt_ofs]); switch (fmt) { case 'b': { port->printf_P(PSTR("%d"), (int)pkt[ofs]); ofs += 1; break; } case 'B': { port->printf_P(PSTR("%u"), (unsigned)pkt[ofs]); ofs += 1; break; } case 'h': { int16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%d"), (int)v); ofs += sizeof(v); break; } case 'H': { uint16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%u"), (unsigned)v); ofs += sizeof(v); break; } case 'i': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%ld"), (long)v); ofs += sizeof(v); break; } case 'I': { uint32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%lu"), (unsigned long)v); ofs += sizeof(v); break; } case 'q': { int64_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%lld"), (long long)v); ofs += sizeof(v); break; } case 'Q': { uint64_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%llu"), (unsigned long long)v); ofs += sizeof(v); break; } case 'f': { float v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%f"), (double)v); ofs += sizeof(v); break; } case 'd': { double v; memcpy(&v, &pkt[ofs], sizeof(v)); // note that %f here *really* means a single-precision // float, so we lose precision printing this double out // dtoa_engine needed.... port->printf_P(PSTR("%f"), (double)v); ofs += sizeof(v); break; } case 'c': { int16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'C': { uint16_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'e': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'E': { uint32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); port->printf_P(PSTR("%.2f"), (double)(0.01f*v)); ofs += sizeof(v); break; } case 'L': { int32_t v; memcpy(&v, &pkt[ofs], sizeof(v)); print_latlon(port, v); ofs += sizeof(v); break; } case 'n': { char v[5]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'N': { char v[17]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'Z': { char v[65]; memcpy(&v, &pkt[ofs], sizeof(v)); v[sizeof(v)-1] = 0; port->printf_P(PSTR("%s"), v); ofs += sizeof(v)-1; break; } case 'M': { print_mode(port, pkt[ofs]); ofs += 1; break; } default: ofs = msg_len; break; } if (ofs < msg_len) { port->printf_P(PSTR(", ")); } } port->println(); }