dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #987 from PX4/gps_logging_dual 

Gps logging dual
This commit is contained in:
Lorenz Meier 2014-05-23 06:09:02 -07:00
parent a14dfc5063 c4d79b84b4
commit 18994ccb72
15 changed files with 276 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/rc.logging
View File

@ -11,6 +11,6 @@ then
sdlog2 start -r 50 -a -b 4 -t
else
echo "Start sdlog2 at 200Hz"
sdlog2 start -r 200 -a -b 16 -t
sdlog2 start -r 200 -a -b 22 -t
fi
fi

6
src/drivers/gps/gps.cpp
View File

@ -448,10 +448,10 @@ GPS::print_info()
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
if (_report.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m);
warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate);

4
src/drivers/gps/gps_helper.cpp
View File

@ -56,7 +56,7 @@ GPS_Helper::get_velocity_update_rate()
return _rate_vel;
}
float
void
GPS_Helper::reset_update_rates()
{
_rate_count_vel = 0;
@ -64,7 +64,7 @@ GPS_Helper::reset_update_rates()
_interval_rate_start = hrt_absolute_time();
}
float
void
GPS_Helper::store_update_rates()
{
_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);

8
src/drivers/gps/gps_helper.h
View File

@ -46,13 +46,17 @@
class GPS_Helper
{
public:
GPS_Helper() {};
virtual ~GPS_Helper() {};
virtual int configure(unsigned &baud) = 0;
virtual int receive(unsigned timeout) = 0;
int set_baudrate(const int &fd, unsigned baud);
float get_position_update_rate();
float get_velocity_update_rate();
float reset_update_rates();
float store_update_rates();
void reset_update_rates();
void store_update_rates();
protected:
uint8_t _rate_count_lat_lon;

47
src/drivers/gps/ubx.cpp
View File

@ -164,7 +164,7 @@ UBX::configure(unsigned &baudrate)
send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: configuration failed: RATE");
warnx("CFG FAIL: RATE");
return 1;
}
@ -185,7 +185,7 @@ UBX::configure(unsigned &baudrate)
send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: configuration failed: NAV5");
warnx("CFG FAIL: NAV5");
return 1;
}
@ -194,35 +194,42 @@ UBX::configure(unsigned &baudrate)
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV POSLLH");
warnx("MSG CFG FAIL: NAV POSLLH");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
warnx("MSG CFG FAIL: NAV TIMEUTC");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV SOL");
warnx("MSG CFG FAIL: NAV SOL");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV VELNED");
warnx("MSG CFG FAIL: NAV VELNED");
return 1;
}
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("ubx: msg rate configuration failed: NAV SVINFO");
warnx("MSG CFG FAIL: NAV SVINFO");
return 1;
}
configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: MON HW");
return 1;
}
@ -274,7 +281,7 @@ UBX::receive(unsigned timeout)
if (ret < 0) {
/* something went wrong when polling */
warnx("ubx: poll error");
warnx("poll error");
return -1;
} else if (ret == 0) {
@ -310,7 +317,7 @@ UBX::receive(unsigned timeout)
/* abort after timeout if no useful packets received */
if (time_started + timeout * 1000 < hrt_absolute_time()) {
warnx("ubx: timeout - no useful messages");
warnx("timeout - no useful messages");
return -1;
}
}
@ -383,7 +390,7 @@ UBX::parse_char(uint8_t b)
return 1; // message received successfully
} else {
warnx("ubx: checksum wrong");
warnx("checksum wrong");
decode_init();
return -1;
}
@ -392,7 +399,7 @@ UBX::parse_char(uint8_t b)
_rx_count++;
} else {
warnx("ubx: buffer full");
warnx("buffer full");
decode_init();
return -1;
}
@ -566,6 +573,24 @@ UBX::handle_message()
break;
}
case UBX_CLASS_MON: {
switch (_message_id) {
case UBX_MESSAGE_MON_HW: {
struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
_gps_position->noise_per_ms = p->noisePerMS;
_gps_position->jamming_indicator = p->jamInd;
ret = 1;
break;
}
default:
break;
}
}
default:
break;
}

28
src/drivers/gps/ubx.h
View File

@ -56,6 +56,7 @@
//#define UBX_CLASS_RXM 0x02
#define UBX_CLASS_ACK 0x05
#define UBX_CLASS_CFG 0x06
#define UBX_CLASS_MON 0x0A
/* MessageIDs (the ones that are used) */
#define UBX_MESSAGE_NAV_POSLLH 0x02
@ -72,6 +73,8 @@
#define UBX_MESSAGE_CFG_RATE 0x08
#define UBX_MESSAGE_CFG_NAV5 0x24
#define UBX_MESSAGE_MON_HW 0x09
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
@ -210,6 +213,27 @@ typedef struct {
uint8_t ck_b;
} gps_bin_nav_velned_packet_t;
struct gps_bin_mon_hw_packet {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t __reserved1;
uint32_t usedMask;
uint8_t VP[25];
uint8_t jamInd;
uint16_t __reserved3;
uint32_t pinIrq;
uint32_t pulLH;
uint32_t pullL;
};
//typedef struct {
// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */
// int16_t week; /**< Measurement GPS week number */
@ -319,7 +343,7 @@ typedef enum {
//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
#pragma pack(pop)
#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer
class UBX : public GPS_Helper
{
@ -383,7 +407,7 @@ private:
uint8_t _message_class;
uint8_t _message_id;
unsigned _payload_size;
uint8_t _disable_cmd_last;
hrt_abstime _disable_cmd_last;
};
#endif /* UBX_H_ */

9
src/modules/commander/commander.cpp
View File

@ -1233,10 +1233,10 @@ int commander_thread_main(int argc, char *argv[])
sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
print_reject_arm("#audio: NOT ARMING: Press safety switch first.");
} else if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[])
home.alt = global_position.alt;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
/* announce new home position */
if (home_pub > 0) {
@ -1848,7 +1848,8 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
/* this needs additional hints to the user - so let other messages pass and be spoken */
mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command);
tune_negative(true);
break;

4
src/modules/commander/state_machine_helper.cpp
View File

@ -138,7 +138,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Allow if HIL_STATE_ON
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first.");
}
valid_transition = false;
@ -312,7 +312,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
case HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
valid_transition = false;
break;

158
src/modules/sdlog2/sdlog2.c
View File

@ -89,6 +89,7 @@
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <version/version.h>
#include <mavlink/mavlink_log.h>
@ -112,12 +113,14 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */
static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */
static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */
static const unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */
static const unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */
static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
static const int MAX_WRITE_CHUNK = 512;
static const int MIN_BYTES_TO_WRITE = 512;
static bool _extended_logging = false;
static const char *log_root = "/fs/microsd/log";
static int mavlink_fd = -1;
struct logbuffer_s lb;
@ -218,6 +221,8 @@ static int create_log_dir(void);
*/
static int open_log_file(void);
static int open_perf_file(const char* str);
static void
sdlog2_usage(const char *reason)
{
@ -225,12 +230,13 @@ sdlog2_usage(const char *reason)
fprintf(stderr, "%s\n", reason);
}
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
"\t-r\tLog rate in Hz, 0 means unlimited rate\n"
"\t-b\tLog buffer size in KiB, default is 8\n"
"\t-e\tEnable logging by default (if not, can be started by command)\n"
"\t-a\tLog only when armed (can be still overriden by command)\n"
"\t-t\tUse date/time for naming log directories and files\n");
"\t-t\tUse date/time for naming log directories and files\n"
"\t-x\tExtended logging");
}
/**
@ -349,8 +355,8 @@ int create_log_dir()
int open_log_file()
{
/* string to hold the path to the log */
char log_file_name[16] = "";
char log_file_path[48] = "";
char log_file_name[32] = "";
char log_file_path[64] = "";
if (log_name_timestamp && gps_time != 0) {
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
@ -378,7 +384,7 @@ int open_log_file()
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
warnx("all %d possible files exist already", MAX_NO_LOGFILE);
mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
return -1;
}
}
@ -387,7 +393,58 @@ int open_log_file()
if (fd < 0) {
warn("failed opening log: %s", log_file_name);
mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
} else {
warnx("log file: %s", log_file_name);
mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
}
return fd;
}
int open_perf_file(const char* str)
{
/* string to hold the path to the log */
char log_file_name[32] = "";
char log_file_path[64] = "";
if (log_name_timestamp && gps_time != 0) {
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
time_t gps_time_sec = gps_time / 1000000;
struct tm t;
gmtime_r(&gps_time_sec, &t);
strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t);
snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
} else {
unsigned file_number = 1; // start with file log001
/* look for the next file that does not exist */
while (file_number <= MAX_NO_LOGFILE) {
/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number);
snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
if (!file_exist(log_file_path)) {
break;
}
file_number++;
}
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
return -1;
}
}
int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
if (fd < 0) {
warn("failed opening log: %s", log_file_name);
mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
} else {
warnx("log file: %s", log_file_name);
@ -529,6 +586,12 @@ void sdlog2_start_log()
errx(1, "error creating logwriter thread");
}
/* write all performance counters */
int perf_fd = open_perf_file("preflight");
dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n");
perf_print_all(perf_fd);
close(perf_fd);
logging_enabled = true;
}
@ -556,6 +619,12 @@ void sdlog2_stop_log()
logwriter_pthread = 0;
pthread_attr_destroy(&logwriter_attr);
/* write all performance counters */
int perf_fd = open_perf_file("postflight");
dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n");
perf_print_all(perf_fd);
close(perf_fd);
sdlog2_status();
}
@ -572,7 +641,7 @@ int write_formats(int fd)
int written = 0;
/* fill message format packet for each format and write it */
for (int i = 0; i < log_formats_num; i++) {
for (unsigned i = 0; i < log_formats_num; i++) {
log_msg_format.body = log_formats[i];
written += write(fd, &log_msg_format, sizeof(log_msg_format));
}
@ -679,7 +748,7 @@ int sdlog2_thread_main(int argc, char *argv[])
* set error flag instead */
bool err_flag = false;
while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
while ((ch = getopt(argc, argv, "r:b:eatx")) != EOF) {
switch (ch) {
case 'r': {
unsigned long r = strtoul(optarg, NULL, 10);
@ -715,6 +784,10 @@ int sdlog2_thread_main(int argc, char *argv[])
log_name_timestamp = true;
break;
case 'x':
_extended_logging = true;
break;
case '?':
if (optopt == 'c') {
warnx("option -%c requires an argument", optopt);
@ -834,8 +907,10 @@ int sdlog2_thread_main(int argc, char *argv[])
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;
struct log_GS0A_s log_GS0A;
struct log_GS0B_s log_GS0B;
struct log_GS1A_s log_GS1A;
struct log_GS1B_s log_GS1B;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@ -969,8 +1044,17 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(STAT);
}
/* --- GPS POSITION --- */
/* --- GPS POSITION - UNIT #1 --- */
if (gps_pos_updated) {
float snr_mean = 0.0f;
for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) {
snr_mean += buf_gps_pos.satellite_snr[i];
}
snr_mean /= buf_gps_pos.satellites_visible;
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;
@ -983,19 +1067,48 @@ int sdlog2_thread_main(int argc, char *argv[])
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;
log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible;
log_msg.body.log_GPS.snr_mean = snr_mean;
log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms;
log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator;
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;
if (_extended_logging) {
/* log the SNR of each satellite for a detailed view of signal quality */
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_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
for (unsigned i = 0; i < sat_max_snr; i++) {
log_msg.body.log_GSN0.satellite_snr[i] = buf_gps_pos.satellite_snr[i];
log_msg.msg_type = LOG_GS0A_MSG;
memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A));
/* fill set A */
for (unsigned i = 0; i < gps_msg_max_snr; i++) {
int satindex = buf_gps_pos.satellite_prn[i] - 1;
/* handles index exceeding and wraps to to arithmetic errors */
if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
/* map satellites by their ID so that logs from two receivers can be compared */
log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
}
}
LOGBUFFER_WRITE_AND_COUNT(GS0A);
log_msg.msg_type = LOG_GS0B_MSG;
memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B));
/* fill set B */
for (unsigned i = 0; i < gps_msg_max_snr; i++) {
/* get second bank of satellites, thus deduct bank size from index */
int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr;
/* handles index exceeding and wraps to to arithmetic errors */
if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
/* map satellites by their ID so that logs from two receivers can be compared */
log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
}
}
LOGBUFFER_WRITE_AND_COUNT(GS0B);
}
LOGBUFFER_WRITE_AND_COUNT(GSN0);
}
/* --- SENSOR COMBINED --- */
@ -1340,6 +1453,7 @@ void sdlog2_status()
float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF");
mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
}

40
src/modules/sdlog2/sdlog2_messages.h
View File

@ -139,6 +139,10 @@ struct log_GPS_s {
float vel_e;
float vel_d;
float cog;
uint8_t sats;
uint16_t snr_mean;
uint16_t noise_per_ms;
uint16_t jamming_indicator;
};
/* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
@ -318,16 +322,28 @@ struct log_VICN_s {
float yaw;
};
/* --- GSN0 - GPS SNR #0 --- */
#define LOG_GSN0_MSG 26
struct log_GSN0_s {
uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
/* --- GS0A - GPS SNR #0, SAT GROUP A --- */
#define LOG_GS0A_MSG 26
struct log_GS0A_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- GSN1 - GPS SNR #1 --- */
#define LOG_GSN1_MSG 27
struct log_GSN1_s {
uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
/* --- GS0B - GPS SNR #0, SAT GROUP B --- */
#define LOG_GS0B_MSG 27
struct log_GS0B_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- GS1A - GPS SNR #1, SAT GROUP A --- */
#define LOG_GS1A_MSG 28
struct log_GS1A_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- GS1B - GPS SNR #1, SAT GROUP B --- */
#define LOG_GS1B_MSG 29
struct log_GS1B_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
@ -363,7 +379,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
@ -381,8 +397,10 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
LOG_FORMAT(GSN0, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GSN1, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */

16
src/modules/systemlib/perf_counter.c
View File

@ -281,13 +281,19 @@ perf_reset(perf_counter_t handle)
void
perf_print_counter(perf_counter_t handle)
{
perf_print_counter_fd(0, handle);
}
void
perf_print_counter_fd(int fd, perf_counter_t handle)
{
if (handle == NULL)
return;
switch (handle->type) {
case PC_COUNT:
printf("%s: %llu events\n",
dprintf(fd, "%s: %llu events\n",
handle->name,
((struct perf_ctr_count *)handle)->event_count);
break;
@ -295,7 +301,7 @@ perf_print_counter(perf_counter_t handle)
case PC_ELAPSED: {
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
handle->name,
pce->event_count,
pce->time_total,
@ -308,7 +314,7 @@ perf_print_counter(perf_counter_t handle)
case PC_INTERVAL: {
struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
printf("%s: %llu events, %llu avg, min %lluus max %lluus\n",
dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n",
handle->name,
pci->event_count,
(pci->time_last - pci->time_first) / pci->event_count,
@ -349,12 +355,12 @@ perf_event_count(perf_counter_t handle)
}
void
perf_print_all(void)
perf_print_all(int fd)
{
perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
while (handle != NULL) {
perf_print_counter(handle);
perf_print_counter_fd(fd, handle);
handle = (perf_counter_t)sq_next(&handle->link);
}
}

16
src/modules/systemlib/perf_counter.h
View File

@ -121,16 +121,26 @@ __EXPORT extern void perf_cancel(perf_counter_t handle);
__EXPORT extern void perf_reset(perf_counter_t handle);
/**
* Print one performance counter.
* Print one performance counter to stdout
*
* @param handle The counter to print.
*/
__EXPORT extern void perf_print_counter(perf_counter_t handle);
/**
* Print all of the performance counters.
* Print one performance counter to a fd.
*
* @param fd File descriptor to print to - e.g. 0 for stdout
* @param handle The counter to print.
*/
__EXPORT extern void perf_print_all(void);
__EXPORT extern void perf_print_counter_fd(int fd, perf_counter_t handle);
/**
* Print all of the performance counters.
*
* @param fd File descriptor to print to - e.g. 0 for stdout
*/
__EXPORT extern void perf_print_all(int fd);
/**
* Reset all of the performance counters.

5
src/modules/uORB/topics/vehicle_gps_position.h
View File

@ -68,6 +68,9 @@ struct vehicle_gps_position_s {
float eph_m; /**< GPS HDOP horizontal dilution of position in m */
float epv_m; /**< GPS VDOP horizontal dilution of position in m */
unsigned noise_per_ms; /**< */
unsigned jamming_indicator; /**< */
uint64_t timestamp_velocity; /**< Timestamp for velocity informations */
float vel_m_s; /**< GPS ground speed (m/s) */
float vel_n_m_s; /**< GPS ground speed in m/s */
@ -85,7 +88,7 @@ struct vehicle_gps_position_s {
uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */
uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */
uint8_t satellite_snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
bool satellite_info_available; /**< 0 for no info, 1 for info available */
};

2
src/systemcmds/perf/perf.c
View File

@ -73,7 +73,7 @@ int perf_main(int argc, char *argv[])
return -1;
}
perf_print_all();
perf_print_all(0 /* stdout */);
fflush(stdout);
return 0;
}

2
src/systemcmds/tests/tests_main.c
View File

@ -235,7 +235,7 @@ test_perf(int argc, char *argv[])
printf("perf: expect count of 1\n");
perf_print_counter(ec);
printf("perf: expect at least two counters\n");
perf_print_all();
perf_print_all(0);
perf_free(cc);
perf_free(ec);