Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 18:52:46 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/PX4/Firmware into fw_control

Browse Source
This commit is contained in:
Thomas Gubler
2012-11-13 20:26:27 +01:00
parent c1e28f5f13 a6294be6f0
commit 5ea79ad1b9
13 changed files with 335 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/logging/logconv.m
+221 -151
View File
@@ -4,162 +4,232 @@ close all
%%%%%%%%%%%%%%%%%%%%%%%
% SYSTEM VECTOR
%
% All measurements in NED frame
%
% uint64_t timestamp;
% float gyro[3]; in rad/s
% float accel[3]; in m/s^2
% float mag[3]; in Gauss
% float baro; pressure in millibar
% float baro_alt; altitude above MSL in meters
% float baro_temp; in degrees celcius
% float control[4]; roll, pitch, yaw [-1..1], thrust [0..1]
% float actuators[8]; motor 1-8, in motor units (PWM: 1000-2000,
% AR.Drone: 0-512
% float vbat; battery voltage in volt
% float adc[3]; remaining auxiliary ADC ports in volt
% float local_position
% int32 gps_raw_position
% //All measurements in NED frame
%
% uint64_t timestamp; //[us]
% float gyro[3]; //[rad/s]
% float accel[3]; //[m/s^2]
% float mag[3]; //[gauss]
% float baro; //pressure [millibar]
% float baro_alt; //altitude above MSL [meter]
% float baro_temp; //[degree celcius]
% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
% float vbat; //battery voltage in [volt]
% float adc[3]; //remaining auxiliary ADC ports [volt]
% float local_position[3]; //tangent plane mapping into x,y,z [m]
% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
% float attitude[3]; //pitch, roll, yaw [rad]
% float rotMatrix[9]; //unitvectors
%myPath = '..\LOG30102012\session0002\'; %set relative path here
myPath = '.\';
myFile = 'sysvector.bin';
filePath = strcat(myPath,myFile);
if exist('sysvector.bin', 'file')
% Read actuators file
myFile = java.io.File('sysvector.bin')
fileSize = length(myFile)
fid = fopen('sysvector.bin', 'r');
elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
for i=1:elements
% timestamp
sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% actuators 1-16
% quadrotor: motor 1-4 on the first four positions
sysvector(i, 2:32) = fread(fid, 28+3, 'float', 'ieee-le');
sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
end
sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
sysvector_minutes = sysvector_interval_seconds / 60
% Normalize time
sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
% Create some basic plots
% Remove zero rows from GPS
gps = sysvector(:,33:35);
gps(~any(gps,2), :) = [];
all_data = figure('Name', 'GPS RAW');
gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
all_data = figure('Name', 'Complete Log Data (exc. GPS)');
plot(sysvector(:,1), sysvector(:,2:32));
actuator_inputs = figure('Name', 'Attitude controller outputs');
plot(sysvector(:,1), sysvector(:,14:17));
legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
actuator_outputs = figure('Name', 'Actuator outputs');
plot(sysvector(:,1), sysvector(:,18:25));
legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
end
if exist('actuator_outputs0.bin', 'file')
% Read actuators file
myFile = java.io.File('actuator_outputs0.bin')
fileSize = length(myFile)
fid = fopen('actuator_outputs0.bin', 'r');
elements = int64(fileSize./(16*4+8))
for i=1:elements
% timestamp
actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% actuators 1-16
% quadrotor: motor 1-4 on the first four positions
actuators(i, 2:17) = fread(fid, 16, 'float', 'ieee-le');
end
end
if exist('actuator_controls0.bin', 'file')
% Read actuators file
myFile = java.io.File('actuator_controls0.bin')
fileSize = length(myFile)
fid = fopen('actuator_controls0.bin', 'r');
elements = int64(fileSize./(8*4+8))
for i=1:elements
% timestamp
actuator_controls(i,1) = fread(fid, 1, 'uint64', 0, 'ieee-le.l64');
% actuators 1-16
% quadrotor: motor 1-4 on the first four positions
actuator_controls(i, 2:9) = fread(fid, 8, 'float', 'ieee-le');
end
end
if exist('sensor_combined.bin', 'file')
% Read sensor combined file
% Type definition: Firmware/apps/uORB/topics/sensor_combined.h
% Struct: sensor_combined_s
fileInfo = dir('sensor_combined.bin');
if exist(filePath, 'file')
fileInfo = dir(filePath);
fileSize = fileInfo.bytes;
fid = fopen('sensor_combined.bin', 'r');
for i=1:elements
fid = fopen(filePath, 'r');
elements = int64(fileSize./(16*4+8))/4
for i=1:elements
% timestamp
sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% gyro raw
sensors(i,2:4) = fread(fid, 3, 'int16', 0, 'ieee-le');
% gyro counter
sensors(i,5) = fread(fid, 1, 'uint16', 0, 'ieee-le');
% gyro in rad/s
sensors(i,6:8) = fread(fid, 3, 'float', 0, 'ieee-le');
% accelerometer raw
sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le');
% padding bytes
fread(fid, 1, 'int16', 0, 'ieee-le');
% accelerometer counter
sensors(i,12) = fread(fid, 1, 'uint32', 0, 'ieee-le');
% accel in m/s2
sensors(i,13:15) = fread(fid, 3, 'float', 0, 'ieee-le');
% accel mode
sensors(i,16) = fread(fid, 1, 'int32', 0, 'ieee-le');
% accel range
sensors(i,17) = fread(fid, 1, 'float', 0, 'ieee-le');
% mag raw
sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le');
% padding bytes
fread(fid, 1, 'int16', 0, 'ieee-le');
% mag in Gauss
sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le');
% mag mode
sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le');
% mag range
sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le');
% mag cuttoff freq
% gyro (3 channels)
sensors(i,2:4) = fread(fid, 3, 'float', 0, 'ieee-le');
% accelerometer (3 channels)
sensors(i,5:7) = fread(fid, 3, 'float', 0, 'ieee-le');
% mag (3 channels)
sensors(i,8:10) = fread(fid, 3, 'float', 0, 'ieee-le');
% baro pressure
sensors(i,11) = fread(fid, 1, 'float', 0, 'ieee-le');
% baro alt
sensors(i,12) = fread(fid, 1, 'float', 0, 'ieee-le');
% baro temp
sensors(i,13) = fread(fid, 1, 'float', 0, 'ieee-le');
% actuator control (4 channels)
sensors(i,14:17) = fread(fid, 4, 'float', 0, 'ieee-le');
% actuator outputs (8 channels)
sensors(i,18:25) = fread(fid, 8, 'float', 0, 'ieee-le');
% vbat
sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
% mag counter
sensors(i,27) = fread(fid, 1, 'int32', 0, 'ieee-le');
% baro pressure millibar
% baro alt meter
% baro temp celcius
% battery voltage
% adc voltage (3 channels)
sensors(i,28:34) = fread(fid, 7, 'float', 0, 'ieee-le');
% baro counter and battery counter
sensors(i,35:36) = fread(fid, 2, 'uint32', 0, 'ieee-le');
% battery voltage valid flag
sensors(i,37) = fread(fid, 1, 'uint32', 0, 'ieee-le');
sensors(i,27:29) = fread(fid, 3, 'float', 0, 'ieee-le');
% local position (3 channels)
sensors(i,30:32) = fread(fid, 3, 'float', 0, 'ieee-le');
% gps_raw_position (3 channels)
sensors(i,33:35) = fread(fid, 3, 'uint32', 0, 'ieee-le');
% attitude (3 channels)
sensors(i,36:38) = fread(fid, 3, 'float', 0, 'ieee-le');
% RotMatrix (9 channels)
sensors(i,39:47) = fread(fid, 9, 'float', 0, 'ieee-le');
end
end
time_us = sensors(elements,1) - sensors(1,1);
time_s = time_us*1e-6
time_m = time_s/60
disp(['end log2matlab conversion' char(10)]);
else
disp(['file: ' filePath ' does not exist' char(10)]);
end
%% old version of reading in different files from sdlog.c
% if exist('sysvector.bin', 'file')
% % Read actuators file
% myFile = java.io.File('sysvector.bin')
% fileSize = length(myFile)
%
% fid = fopen('sysvector.bin', 'r');
% elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
%
% for i=1:elements
% % timestamp
% sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% % actuators 1-16
% % quadrotor: motor 1-4 on the first four positions
% sysvector(i, 2:32) = fread(fid, 28+3, 'float', 'ieee-le');
% sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
% end
%
% sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
% sysvector_minutes = sysvector_interval_seconds / 60
%
% % Normalize time
% sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
%
% % Create some basic plots
%
% % Remove zero rows from GPS
% gps = sysvector(:,33:35);
% gps(~any(gps,2), :) = [];
%
% all_data = figure('Name', 'GPS RAW');
% gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
%
%
% all_data = figure('Name', 'Complete Log Data (exc. GPS)');
% plot(sysvector(:,1), sysvector(:,2:32));
%
% actuator_inputs = figure('Name', 'Attitude controller outputs');
% plot(sysvector(:,1), sysvector(:,14:17));
% legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
%
% actuator_outputs = figure('Name', 'Actuator outputs');
% plot(sysvector(:,1), sysvector(:,18:25));
% legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
%
% end
%
% if exist('actuator_outputs0.bin', 'file')
% % Read actuators file
% myFile = java.io.File('actuator_outputs0.bin')
% fileSize = length(myFile)
%
% fid = fopen('actuator_outputs0.bin', 'r');
% elements = int64(fileSize./(16*4+8))
%
% for i=1:elements
% % timestamp
% actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% % actuators 1-16
% % quadrotor: motor 1-4 on the first four positions
% actuators(i, 2:17) = fread(fid, 16, 'float', 'ieee-le');
% end
% end
%
% if exist('actuator_controls0.bin', 'file')
% % Read actuators file
% myFile = java.io.File('actuator_controls0.bin')
% fileSize = length(myFile)
%
% fid = fopen('actuator_controls0.bin', 'r');
% elements = int64(fileSize./(8*4+8))
%
% for i=1:elements
% % timestamp
% actuator_controls(i,1) = fread(fid, 1, 'uint64', 0, 'ieee-le.l64');
% % actuators 1-16
% % quadrotor: motor 1-4 on the first four positions
% actuator_controls(i, 2:9) = fread(fid, 8, 'float', 'ieee-le');
% end
% end
%
%
% if exist('sensor_combined.bin', 'file')
% % Read sensor combined file
% % Type definition: Firmware/apps/uORB/topics/sensor_combined.h
% % Struct: sensor_combined_s
% fileInfo = dir('sensor_combined.bin');
% fileSize = fileInfo.bytes;
%
% fid = fopen('sensor_combined.bin', 'r');
%
% for i=1:elements
% % timestamp
% sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% % gyro raw
% sensors(i,2:4) = fread(fid, 3, 'int16', 0, 'ieee-le');
% % gyro counter
% sensors(i,5) = fread(fid, 1, 'uint16', 0, 'ieee-le');
% % gyro in rad/s
% sensors(i,6:8) = fread(fid, 3, 'float', 0, 'ieee-le');
%
% % accelerometer raw
% sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le');
% % padding bytes
% fread(fid, 1, 'int16', 0, 'ieee-le');
% % accelerometer counter
% sensors(i,12) = fread(fid, 1, 'uint32', 0, 'ieee-le');
% % accel in m/s2
% sensors(i,13:15) = fread(fid, 3, 'float', 0, 'ieee-le');
% % accel mode
% sensors(i,16) = fread(fid, 1, 'int32', 0, 'ieee-le');
% % accel range
% sensors(i,17) = fread(fid, 1, 'float', 0, 'ieee-le');
%
% % mag raw
% sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le');
% % padding bytes
% fread(fid, 1, 'int16', 0, 'ieee-le');
% % mag in Gauss
% sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le');
% % mag mode
% sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le');
% % mag range
% sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le');
% % mag cuttoff freq
% sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
% % mag counter
% sensors(i,27) = fread(fid, 1, 'int32', 0, 'ieee-le');
%
% % baro pressure millibar
% % baro alt meter
% % baro temp celcius
% % battery voltage
% % adc voltage (3 channels)
% sensors(i,28:34) = fread(fid, 7, 'float', 0, 'ieee-le');
% % baro counter and battery counter
% sensors(i,35:36) = fread(fid, 2, 'uint32', 0, 'ieee-le');
% % battery voltage valid flag
% sensors(i,37) = fread(fid, 1, 'uint32', 0, 'ieee-le');
%
% end
% end
apps/commander/commander.c
-1
View File
@@ -149,7 +149,6 @@ static void led_deinit(void);
static int led_toggle(int led);
static int led_on(int led);
static int led_off(int led);
static int pm_save_eeprom(bool only_unsaved);
static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
static void do_accel_calibration(int status_pub, struct vehicle_status_s *status);
apps/commander/state_machine_helper.c
+2
View File
@@ -579,6 +579,8 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
state_machine_publish(status_pub, current_status, mavlink_fd);
publish_armed_status(current_status);
printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
} else if (current_status->state_machine != SYSTEM_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.")
}
/* NEVER actually switch off HIL without reboot */
apps/drivers/hil/hil.cpp
+3 -1
View File
@@ -474,6 +474,7 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg)
ret = HIL::pwm_ioctl(filp, cmd, arg);
break;
default:
ret = -ENOTTY;
debug("not in a PWM mode");
break;
}
@@ -489,7 +490,7 @@ int
HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
// int channel;
lock();
@@ -824,6 +825,7 @@ hil_main(int argc, char *argv[])
// XXX all modes have PWM settings
if (argc > i + 1) {
pwm_update_rate_in_hz = atoi(argv[i + 1]);
printf("pwm update rate: %d Hz\n", pwm_update_rate_in_hz);
} else {
fprintf(stderr, "missing argument for pwm update rate (-u)\n");
return 1;
apps/drivers/px4io/px4io.cpp
+1
View File
@@ -467,6 +467,7 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
/* publish raw rc channel values from IO */
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
for (int i = 0; i < rep->channel_count; i++)
{
_input_rc.values[i] = rep->rc_channel[i];
apps/gps/mtk.c
+4 -3
View File
@@ -418,10 +418,11 @@ void *mtk_watchdog_loop(void *args)
mtk_gps->satellite_info_available = 0;
// global_data_send_subsystem_info(&mtk_present_enabled_healthy);
mavlink_log_info(mavlink_fd, "[gps] MTK custom binary module found, status ok\n");
mtk_healthy = true;
mtk_fail_count = 0;
once_ok = true;
}
mtk_healthy = true;
mtk_fail_count = 0;
once_ok = true;
}
usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS);
apps/gps/ubx.c
+6 -5
View File
@@ -272,7 +272,8 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
ubx_gps->s_variance = packet->sAcc;
ubx_gps->p_variance = packet->pAcc;
//pthread_mutex_lock(ubx_mutex);
ubx_state->last_message_timestamps[NAV_SOL - 1] = hrt_absolute_time();
@@ -785,10 +786,6 @@ void *ubx_watchdog_loop(void *args)
sleep(1);
} else {
/* gps healthy */
ubx_success_count++;
ubx_healthy = true;
ubx_fail_count = 0;
if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
@@ -799,6 +796,10 @@ void *ubx_watchdog_loop(void *args)
once_ok = true;
}
/* gps healthy */
ubx_success_count++;
ubx_healthy = true;
ubx_fail_count = 0;
}
usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS);
apps/mavlink/mavlink.c
+2 -2
View File
@@ -163,8 +163,8 @@ set_hil_on_off(bool hil_enabled)
/* 20 Hz */
hil_rate_interval = 50;
} else {
/* 100 Hz */
hil_rate_interval = 10;
/* 200 Hz */
hil_rate_interval = 5;
}
orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
apps/sdlog/sdlog.c
+22 -17
View File
@@ -488,23 +488,25 @@ int sdlog_thread_main(int argc, char *argv[]) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
#pragma pack(push, 1)
struct {
uint64_t timestamp;
float gyro[3];
float accel[3];
float mag[3];
float baro;
float baro_alt;
float baro_temp;
float control[4];
float actuators[8];
float vbat;
float adc[3];
float local_pos[3];
int32_t gps_pos[3];
uint64_t timestamp; //[us]
float gyro[3]; //[rad/s]
float accel[3]; //[m/s^2]
float mag[3]; //[gauss]
float baro; //pressure [millibar]
float baro_alt; //altitude above MSL [meter]
float baro_temp; //[degree celcius]
float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
float vbat; //battery voltage in [volt]
float adc[3]; //remaining auxiliary ADC ports [volt]
float local_position[3]; //tangent plane mapping into x,y,z [m]
int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
float attitude[3]; //pitch, roll, yaw [rad]
float rotMatrix[9]; //unitvectors
} sysvector = {
.timestamp = buf.raw.timestamp,
.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
@@ -518,14 +520,16 @@ int sdlog_thread_main(int argc, char *argv[]) {
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
.vbat = buf.raw.battery_voltage_v,
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
.local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
.gps_pos = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}
};
#pragma pack(pop)
sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));
usleep(10000);
usleep(10000); //10000 corresponds in reality to ca. 76 Hz
}
fsync(sysvector_file);
@@ -602,3 +606,4 @@ int file_copy(const char* file_old, const char* file_new)
return ret;
}
apps/sensors/sensors.cpp
+5 -1
View File
@@ -93,6 +93,8 @@
#define BAT_VOL_LOWPASS_2 0.01f
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */
/**
* Sensor app start / stop handling function
*
@@ -865,8 +867,10 @@ Sensors::ppm_poll()
struct rc_input_values raw;
raw.timestamp = ppm_last_valid_decode;
/* we are accepting this message */
_ppm_last_valid = ppm_last_valid_decode;
if (ppm_decoded_channels > 1) {
if (ppm_decoded_channels > 2 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
for (int i = 0; i < ppm_decoded_channels; i++) {
raw.values[i] = ppm_buffer[i];
apps/systemlib/perf_counter.c
+65 -5
View File
@@ -73,6 +73,20 @@ struct perf_ctr_elapsed {
uint64_t time_most;
};
/**
* PC_INTERVAL counter.
*/
struct perf_ctr_interval {
struct perf_ctr_header hdr;
uint64_t event_count;
uint64_t time_event;
uint64_t time_first;
uint64_t time_last;
uint64_t time_least;
uint64_t time_most;
};
/**
* List of all known counters.
*/
@@ -93,6 +107,10 @@ perf_alloc(enum perf_counter_type type, const char *name)
ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1);
break;
case PC_INTERVAL:
ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
break;
default:
break;
}
@@ -127,6 +145,32 @@ perf_count(perf_counter_t handle)
((struct perf_ctr_count *)handle)->event_count++;
break;
case PC_INTERVAL: {
struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
hrt_abstime now = hrt_absolute_time();
switch (pci->event_count) {
case 0:
pci->time_first = now;
break;
case 1:
pci->time_least = now - pci->time_last;
pci->time_most = now - pci->time_last;
break;
default: {
hrt_abstime interval = now - pci->time_last;
if (interval < pci->time_least)
pci->time_least = interval;
if (interval > pci->time_most)
pci->time_most = interval;
break;
}
}
pci->time_last = now;
pci->event_count++;
break;
}
default:
break;
}
@@ -187,13 +231,29 @@ perf_print_counter(perf_counter_t handle)
((struct perf_ctr_count *)handle)->event_count);
break;
case PC_ELAPSED:
case PC_ELAPSED: {
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n",
handle->name,
((struct perf_ctr_elapsed *)handle)->event_count,
((struct perf_ctr_elapsed *)handle)->time_total,
((struct perf_ctr_elapsed *)handle)->time_least,
((struct perf_ctr_elapsed *)handle)->time_most);
pce->event_count,
pce->time_total,
pce->time_least,
pce->time_most);
break;
}
case PC_INTERVAL: {
struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
printf("%s: %llu events, %llu avg, min %lluus max %lluus\n",
handle->name,
pci->event_count,
(pci->time_last - pci->time_first) / pci->event_count,
pci->time_least,
pci->time_most);
break;
}
default:
break;
apps/systemlib/perf_counter.h
+2 -1
View File
@@ -44,7 +44,8 @@
*/
enum perf_counter_type {
PC_COUNT, /**< count the number of times an event occurs */
PC_ELAPSED /**< measure the time elapsed performing an event */
PC_ELAPSED, /**< measure the time elapsed performing an event */
PC_INTERVAL /**< measure the interval between instances of an event */
};
struct perf_ctr_header;
apps/uORB/topics/vehicle_gps_position.h
+2
View File
@@ -65,6 +65,8 @@ struct vehicle_gps_position_s
uint16_t counter_pos_valid; /**< is only increased when new lat/lon/alt information was added */
uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */
uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
float s_variance; // XXX testing
float p_variance; // XXX testing
uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */
float vel_n; /**< GPS ground speed in m/s */
float vel_e; /**< GPS ground speed in m/s */