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();
}
