mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 22:43:33 +08:00
Merge branch 'beta' into stable
This commit is contained in:
Lorenz Meier
+1
-1
Submodule NuttX updated: 678eea3d2c...8891d035df
@@ -3,16 +3,50 @@
|
||||
# UAVCAN initialization script.
|
||||
#
|
||||
|
||||
#
|
||||
# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable.
|
||||
# TODO there should be a smarter way.
|
||||
#
|
||||
set UAVCAN_ENABLE 0
|
||||
if param compare UAVCAN_ENABLE 1
|
||||
then
|
||||
set UAVCAN_ENABLE 1
|
||||
fi
|
||||
if param compare UAVCAN_ENABLE 2
|
||||
then
|
||||
set UAVCAN_ENABLE 2
|
||||
fi
|
||||
|
||||
echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE"
|
||||
|
||||
#
|
||||
# Starting stuff according to UAVCAN_ENABLE value
|
||||
#
|
||||
if [ $UAVCAN_ENABLE -ge 1 ]
|
||||
then
|
||||
if uavcan start
|
||||
then
|
||||
# First sensor publisher to initialize takes lowest instance ID
|
||||
# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
|
||||
sleep 1
|
||||
echo "[i] UAVCAN started"
|
||||
else
|
||||
echo "[i] ERROR: Could not start UAVCAN"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ $UAVCAN_ENABLE -ge 2 ]
|
||||
then
|
||||
if uavcan start fw
|
||||
then
|
||||
echo "[i] UAVCAN servers started"
|
||||
else
|
||||
echo "[i] ERROR: Could not start UAVCAN servers"
|
||||
tone_alarm $TUNE_ERR
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ $UAVCAN_ENABLE -ge 1 ]
|
||||
then
|
||||
# First sensor publisher to initialize takes lowest instance ID
|
||||
# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs
|
||||
sleep 8
|
||||
fi
|
||||
|
||||
@@ -3,22 +3,7 @@
|
||||
# USB MAVLink start
|
||||
#
|
||||
|
||||
mavlink start -r 800000 -d /dev/ttyACM0 -x
|
||||
# Enable a number of interesting streams we want via USB
|
||||
mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300
|
||||
mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
|
||||
mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
|
||||
mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10
|
||||
mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
|
||||
mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100
|
||||
mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30
|
||||
mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
|
||||
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
|
||||
mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
|
||||
mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30
|
||||
mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5
|
||||
mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100
|
||||
mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20
|
||||
mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
|
||||
|
||||
# Exit shell to make it available to MAVLink
|
||||
exit
|
||||
|
||||
@@ -299,6 +299,11 @@ then
|
||||
then
|
||||
fi
|
||||
|
||||
#
|
||||
# UAVCAN
|
||||
#
|
||||
sh /etc/init.d/rc.uavcan
|
||||
|
||||
#
|
||||
# Sensors System (start before Commander so Preflight checks are properly run)
|
||||
#
|
||||
@@ -491,10 +496,6 @@ then
|
||||
fi
|
||||
fi
|
||||
|
||||
# UAVCAN
|
||||
#
|
||||
sh /etc/init.d/rc.uavcan
|
||||
|
||||
#
|
||||
# Logging
|
||||
#
|
||||
|
||||
Submodule mavlink/include/mavlink/v1.0 updated: d23a551e10...c390a63abf
+7
-4
@@ -18,9 +18,12 @@ float32 airspeed_filtered
|
||||
float32 airspeedDerivativeSp
|
||||
float32 airspeedDerivative
|
||||
|
||||
float32 totalEnergyRateSp
|
||||
float32 totalEnergyRate
|
||||
float32 energyDistributionRateSp
|
||||
float32 energyDistributionRate
|
||||
float32 totalEnergyError
|
||||
float32 energyDistributionError
|
||||
float32 totalEnergyRateError
|
||||
float32 energyDistributionRateError
|
||||
|
||||
float32 throttle_sp
|
||||
float32 pitch_sp
|
||||
|
||||
uint8 mode
|
||||
|
||||
@@ -355,7 +355,7 @@ PX4FMU::init()
|
||||
_task = task_spawn_cmd("fmuservo",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
1600,
|
||||
1200,
|
||||
(main_t)&PX4FMU::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
|
||||
@@ -309,12 +309,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
|
||||
// Calculate total energy values
|
||||
_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
|
||||
float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
|
||||
float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
|
||||
_STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
|
||||
|
||||
// Apply 0.5 second first order filter to STEdot_error
|
||||
// This is required to remove accelerometer noise from the measurement
|
||||
STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast;
|
||||
_STEdotErrLast = STEdot_error;
|
||||
_STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast;
|
||||
_STEdotErrLast = _STEdot_error;
|
||||
|
||||
// Calculate throttle demand
|
||||
// If underspeed condition is set, then demand full throttle
|
||||
@@ -342,7 +342,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
|
||||
}
|
||||
|
||||
// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
|
||||
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
|
||||
_throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
|
||||
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
|
||||
|
||||
// Rate limit PD + FF throttle
|
||||
@@ -438,11 +438,11 @@ void TECS::_update_pitch(void)
|
||||
// Calculate Specific Energy Balance demand, and error
|
||||
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
|
||||
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
|
||||
float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
|
||||
float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
|
||||
_SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
|
||||
_SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
|
||||
|
||||
// Calculate integrator state, constraining input if pitch limits are exceeded
|
||||
float integ7_input = SEB_error * _integGain;
|
||||
float integ7_input = _SEB_error * _integGain;
|
||||
|
||||
if (_pitch_dem_unc > _PITCHmaxf) {
|
||||
integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
|
||||
@@ -460,7 +460,7 @@ void TECS::_update_pitch(void)
|
||||
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
|
||||
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
|
||||
float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
|
||||
float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
|
||||
float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
|
||||
if (_climbOutDem)
|
||||
{
|
||||
temp += _PITCHminf * gainInv;
|
||||
@@ -598,18 +598,27 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
|
||||
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
|
||||
}
|
||||
|
||||
_tecs_state.hgt_dem = _hgt_dem_adj;
|
||||
_tecs_state.hgt = _integ3_state;
|
||||
_tecs_state.dhgt_dem = _hgt_rate_dem;
|
||||
_tecs_state.dhgt = _integ2_state;
|
||||
_tecs_state.spd_dem = _TAS_dem_adj;
|
||||
_tecs_state.spd = _integ5_state;
|
||||
_tecs_state.dspd = _vel_dot;
|
||||
_tecs_state.ithr = _integ6_state;
|
||||
_tecs_state.iptch = _integ7_state;
|
||||
_tecs_state.thr = _throttle_dem;
|
||||
_tecs_state.ptch = _pitch_dem;
|
||||
_tecs_state.dspd_dem = _TAS_rate_dem;
|
||||
_tecs_state.altitude_sp = _hgt_dem_adj;
|
||||
_tecs_state.altitude_filtered = _integ3_state;
|
||||
_tecs_state.altitude_rate_sp = _hgt_rate_dem;
|
||||
_tecs_state.altitude_rate = _integ2_state;
|
||||
|
||||
_tecs_state.airspeed_sp = _TAS_dem_adj;
|
||||
_tecs_state.airspeed_rate_sp = _TAS_rate_dem;
|
||||
_tecs_state.airspeed_filtered = _integ5_state;
|
||||
_tecs_state.airspeed_rate = _vel_dot;
|
||||
|
||||
_tecs_state.total_energy_error = _STE_error;
|
||||
_tecs_state.energy_distribution_error = _SEB_error;
|
||||
_tecs_state.total_energy_rate_error = _STEdot_error;
|
||||
_tecs_state.energy_distribution_error = _SEBdot_error;
|
||||
|
||||
_tecs_state.energy_error_integ = _integ6_state;
|
||||
_tecs_state.energy_distribution_error_integ = _integ7_state;
|
||||
|
||||
|
||||
_tecs_state.throttle_sp = _throttle_dem;
|
||||
_tecs_state.pitch_sp = _pitch_dem;
|
||||
|
||||
_update_pitch_throttle_last_usec = now;
|
||||
|
||||
|
||||
@@ -79,6 +79,10 @@ public:
|
||||
_SKE_est(0.0f),
|
||||
_SPEdot(0.0f),
|
||||
_SKEdot(0.0f),
|
||||
_STE_error(0.0f),
|
||||
_STEdot_error(0.0f),
|
||||
_SEB_error(0.0f),
|
||||
_SEBdot_error(0.0f),
|
||||
_airspeed_enabled(false),
|
||||
_states_initalized(false),
|
||||
_in_air(false),
|
||||
@@ -137,18 +141,22 @@ public:
|
||||
|
||||
struct tecs_state {
|
||||
uint64_t timestamp;
|
||||
float hgt;
|
||||
float dhgt;
|
||||
float hgt_dem;
|
||||
float dhgt_dem;
|
||||
float spd_dem;
|
||||
float spd;
|
||||
float dspd;
|
||||
float ithr;
|
||||
float iptch;
|
||||
float thr;
|
||||
float ptch;
|
||||
float dspd_dem;
|
||||
float altitude_filtered;
|
||||
float altitude_sp;
|
||||
float altitude_rate;
|
||||
float altitude_rate_sp;
|
||||
float airspeed_filtered;
|
||||
float airspeed_sp;
|
||||
float airspeed_rate;
|
||||
float airspeed_rate_sp;
|
||||
float energy_error_integ;
|
||||
float energy_distribution_error_integ;
|
||||
float total_energy_error;
|
||||
float total_energy_rate_error;
|
||||
float energy_distribution_error;
|
||||
float energy_distribution_rate_error;
|
||||
float throttle_sp;
|
||||
float pitch_sp;
|
||||
enum ECL_TECS_MODE mode;
|
||||
};
|
||||
|
||||
@@ -376,6 +384,15 @@ private:
|
||||
// Specific energy error quantities
|
||||
float _STE_error;
|
||||
|
||||
// Energy error rate
|
||||
float _STEdot_error;
|
||||
|
||||
// Specific energy balance error
|
||||
float _SEB_error;
|
||||
|
||||
// Specific energy balance error rate
|
||||
float _SEBdot_error;
|
||||
|
||||
// Time since last update of main TECS loop (seconds)
|
||||
float _DT;
|
||||
|
||||
|
||||
+1
-1
Submodule src/lib/uavcan updated: 46793c5e06...3ae5400aa5
@@ -1115,7 +1115,7 @@ int AttitudePositionEstimatorEKF::start()
|
||||
_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 40,
|
||||
7500,
|
||||
4800,
|
||||
(main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
|
||||
@@ -42,5 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \
|
||||
estimator_22states.cpp \
|
||||
estimator_utilities.cpp
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100
|
||||
EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3400
|
||||
|
||||
|
||||
@@ -1920,22 +1920,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
|
||||
break;
|
||||
}
|
||||
|
||||
t.altitudeSp = s.hgt_dem;
|
||||
t.altitude_filtered = s.hgt;
|
||||
t.airspeedSp = s.spd_dem;
|
||||
t.airspeed_filtered = s.spd;
|
||||
t.altitudeSp = s.altitude_sp;
|
||||
t.altitude_filtered = s.altitude_filtered;
|
||||
t.airspeedSp = s.airspeed_sp;
|
||||
t.airspeed_filtered = s.airspeed_filtered;
|
||||
|
||||
t.flightPathAngleSp = s.dhgt_dem;
|
||||
t.flightPathAngle = s.dhgt;
|
||||
t.flightPathAngleFiltered = s.dhgt;
|
||||
t.flightPathAngleSp = s.altitude_rate_sp;
|
||||
t.flightPathAngle = s.altitude_rate;
|
||||
t.flightPathAngleFiltered = s.altitude_rate;
|
||||
|
||||
t.airspeedDerivativeSp = s.dspd_dem;
|
||||
t.airspeedDerivative = s.dspd;
|
||||
t.airspeedDerivativeSp = s.airspeed_rate_sp;
|
||||
t.airspeedDerivative = s.airspeed_rate;
|
||||
|
||||
t.totalEnergyRateSp = s.thr;
|
||||
t.totalEnergyRate = s.ithr;
|
||||
t.energyDistributionRateSp = s.ptch;
|
||||
t.energyDistributionRate = s.iptch;
|
||||
t.totalEnergyError = s.total_energy_error;
|
||||
t.totalEnergyRateError = s.total_energy_rate_error;
|
||||
t.energyDistributionError = s.energy_distribution_error;
|
||||
t.energyDistributionRateError = s.energy_distribution_rate_error;
|
||||
|
||||
t.throttle_sp = s.throttle_sp;
|
||||
t.pitch_sp = s.pitch_sp;
|
||||
|
||||
if (_tecs_status_pub > 0) {
|
||||
orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
|
||||
|
||||
@@ -226,17 +226,17 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
|
||||
* is running) */
|
||||
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
|
||||
|
||||
/* Write part of the status message */
|
||||
_status.flightPathAngleSp = flightPathAngleSp;
|
||||
_status.flightPathAngle = flightPathAngle;
|
||||
_status.flightPathAngleFiltered = flightPathAngleFiltered;
|
||||
_status.airspeedDerivativeSp = airspeedDerivativeSp;
|
||||
_status.airspeedDerivative = airspeedDerivative;
|
||||
_status.totalEnergyRateSp = totalEnergyRateSp;
|
||||
_status.totalEnergyRate = totalEnergyRate;
|
||||
_status.energyDistributionRateSp = energyDistributionRateSp;
|
||||
_status.energyDistributionRate = energyDistributionRate;
|
||||
_status.mode = mode;
|
||||
// /* Write part of the status message */
|
||||
// _status.flightPathAngleSp = flightPathAngleSp;
|
||||
// _status.flightPathAngle = flightPathAngle;
|
||||
// _status.flightPathAngleFiltered = flightPathAngleFiltered;
|
||||
// _status.airspeedDerivativeSp = airspeedDerivativeSp;
|
||||
// _status.airspeedDerivative = airspeedDerivative;
|
||||
// _status.totalEnergyRateSp = totalEnergyRateSp;
|
||||
// _status.totalEnergyRate = totalEnergyRate;
|
||||
// _status.energyDistributionRateSp = energyDistributionRateSp;
|
||||
// _status.energyDistributionRate = energyDistributionRate;
|
||||
// _status.mode = mode;
|
||||
|
||||
/** update control blocks **/
|
||||
/* update total energy rate control block */
|
||||
|
||||
@@ -1353,6 +1353,8 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
_mode = MAVLINK_MODE_ONBOARD;
|
||||
} else if (strcmp(optarg, "osd") == 0) {
|
||||
_mode = MAVLINK_MODE_OSD;
|
||||
} else if (strcmp(optarg, "config") == 0) {
|
||||
_mode = MAVLINK_MODE_CONFIG;
|
||||
}
|
||||
|
||||
break;
|
||||
@@ -1449,8 +1451,6 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
/* start the MAVLink receiver */
|
||||
_receive_thread = MavlinkReceiver::receive_start(this);
|
||||
|
||||
_task_running = true;
|
||||
|
||||
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
|
||||
uint64_t param_time = 0;
|
||||
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
|
||||
@@ -1537,6 +1537,23 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
configure_stream("RC_CHANNELS_RAW", 5.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_CONFIG:
|
||||
// Enable a number of interesting streams we want via USB
|
||||
configure_stream("PARAM_VALUE", 300.0f);
|
||||
configure_stream("MISSION_ITEM", 50.0f);
|
||||
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
|
||||
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
|
||||
configure_stream("VFR_HUD", 20.0f);
|
||||
configure_stream("ATTITUDE", 100.0f);
|
||||
configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
|
||||
configure_stream("RC_CHANNELS_RAW", 5.0f);
|
||||
configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
|
||||
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
|
||||
configure_stream("LOCAL_POSITION_NED", 30.0f);
|
||||
configure_stream("MANUAL_CONTROL", 5.0f);
|
||||
configure_stream("HIGHRES_IMU", 100.0f);
|
||||
configure_stream("GPS_RAW_INT", 20.0f);
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@@ -1703,6 +1720,9 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
|
||||
/* confirm task running only once fully initialized */
|
||||
_task_running = true;
|
||||
}
|
||||
|
||||
delete _subscribe_to_stream;
|
||||
|
||||
@@ -129,7 +129,8 @@ public:
|
||||
MAVLINK_MODE_NORMAL = 0,
|
||||
MAVLINK_MODE_CUSTOM,
|
||||
MAVLINK_MODE_ONBOARD,
|
||||
MAVLINK_MODE_OSD
|
||||
MAVLINK_MODE_OSD,
|
||||
MAVLINK_MODE_CONFIG
|
||||
};
|
||||
|
||||
void set_mode(enum MAVLINK_MODE);
|
||||
|
||||
@@ -105,6 +105,7 @@
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/git_version.h>
|
||||
#include <systemlib/printload.h>
|
||||
#include <version/version.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
@@ -727,9 +728,16 @@ void sdlog2_start_log()
|
||||
}
|
||||
|
||||
/* write all performance counters */
|
||||
hrt_abstime curr_time = hrt_absolute_time();
|
||||
struct print_load_s load;
|
||||
int perf_fd = open_perf_file("preflight");
|
||||
init_print_load_s(curr_time, &load);
|
||||
print_load(curr_time, perf_fd, &load);
|
||||
dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n");
|
||||
perf_print_all(perf_fd);
|
||||
dprintf(perf_fd, "\nLOAD PRE-FLIGHT\n\n");
|
||||
usleep(500 * 1000);
|
||||
print_load(hrt_absolute_time(), perf_fd, &load);
|
||||
close(perf_fd);
|
||||
|
||||
/* reset performance counters to get in-flight min and max values in post flight log */
|
||||
@@ -765,8 +773,15 @@ void sdlog2_stop_log()
|
||||
|
||||
/* write all performance counters */
|
||||
int perf_fd = open_perf_file("postflight");
|
||||
hrt_abstime curr_time = hrt_absolute_time();
|
||||
dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n");
|
||||
perf_print_all(perf_fd);
|
||||
struct print_load_s load;
|
||||
dprintf(perf_fd, "\nLOAD POST-FLIGHT\n\n");
|
||||
init_print_load_s(curr_time, &load);
|
||||
print_load(curr_time, perf_fd, &load);
|
||||
sleep(1);
|
||||
print_load(hrt_absolute_time(), perf_fd, &load);
|
||||
close(perf_fd);
|
||||
|
||||
/* free log writer performance counter */
|
||||
@@ -1863,15 +1878,16 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
|
||||
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
|
||||
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
|
||||
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
|
||||
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
|
||||
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
|
||||
log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
|
||||
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
|
||||
log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
|
||||
log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
|
||||
log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
|
||||
log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
|
||||
log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError;
|
||||
log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError;
|
||||
log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError;
|
||||
log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError;
|
||||
log_msg.body.log_TECS.throttle_sp = buf.tecs_status.throttle_sp;
|
||||
log_msg.body.log_TECS.pitch_sp = buf.tecs_status.pitch_sp;
|
||||
log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
|
||||
LOGBUFFER_WRITE_AND_COUNT(TECS);
|
||||
}
|
||||
|
||||
@@ -360,16 +360,16 @@ struct log_TECS_s {
|
||||
float altitudeFiltered;
|
||||
float flightPathAngleSp;
|
||||
float flightPathAngle;
|
||||
float flightPathAngleFiltered;
|
||||
float airspeedSp;
|
||||
float airspeedFiltered;
|
||||
float airspeedDerivativeSp;
|
||||
float airspeedDerivative;
|
||||
|
||||
float totalEnergyRateSp;
|
||||
float totalEnergyRate;
|
||||
float energyDistributionRateSp;
|
||||
float energyDistributionRate;
|
||||
float totalEnergyError;
|
||||
float totalEnergyRateError;
|
||||
float energyDistributionError;
|
||||
float energyDistributionRateError;
|
||||
float throttle_sp;
|
||||
float pitch_sp;
|
||||
|
||||
uint8_t mode;
|
||||
};
|
||||
@@ -527,7 +527,7 @@ static const struct log_format_s log_formats[] = {
|
||||
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"),
|
||||
LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
|
||||
LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,Thr,Ptch,M"),
|
||||
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
|
||||
LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"),
|
||||
LOG_FORMAT(TSYN, "Q", "TimeOffset"),
|
||||
|
||||
@@ -42,6 +42,7 @@ SRCS = err.c \
|
||||
bson/tinybson.c \
|
||||
conversions.c \
|
||||
cpuload.c \
|
||||
printload.c \
|
||||
getopt_long.c \
|
||||
up_cxxinitialize.c \
|
||||
pid/pid.c \
|
||||
|
||||
@@ -0,0 +1,278 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file printload.c
|
||||
*
|
||||
* Print the current system load.
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <systemlib/cpuload.h>
|
||||
#include <systemlib/printload.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
extern struct system_load_s system_load;
|
||||
|
||||
#define CL "\033[K" // clear line
|
||||
|
||||
void init_print_load_s(uint64_t t, struct print_load_s *s)
|
||||
{
|
||||
|
||||
s->total_user_time = 0;
|
||||
|
||||
s->running_count = 0;
|
||||
s->blocked_count = 0;
|
||||
|
||||
s->new_time = t;
|
||||
s->interval_start_time = t;
|
||||
|
||||
for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
|
||||
s->last_times[i] = 0;
|
||||
}
|
||||
|
||||
s->interval_time_ms_inv = 0.f;
|
||||
}
|
||||
|
||||
static const char *
|
||||
tstate_name(const tstate_t s)
|
||||
{
|
||||
switch (s) {
|
||||
case TSTATE_TASK_INVALID:
|
||||
return "init";
|
||||
|
||||
case TSTATE_TASK_PENDING:
|
||||
return "PEND";
|
||||
|
||||
case TSTATE_TASK_READYTORUN:
|
||||
return "READY";
|
||||
|
||||
case TSTATE_TASK_RUNNING:
|
||||
return "RUN";
|
||||
|
||||
case TSTATE_TASK_INACTIVE:
|
||||
return "inact";
|
||||
|
||||
case TSTATE_WAIT_SEM:
|
||||
return "w:sem";
|
||||
#ifndef CONFIG_DISABLE_SIGNALS
|
||||
|
||||
case TSTATE_WAIT_SIG:
|
||||
return "w:sig";
|
||||
#endif
|
||||
#ifndef CONFIG_DISABLE_MQUEUE
|
||||
|
||||
case TSTATE_WAIT_MQNOTEMPTY:
|
||||
return "w:mqe";
|
||||
|
||||
case TSTATE_WAIT_MQNOTFULL:
|
||||
return "w:mqf";
|
||||
#endif
|
||||
#ifdef CONFIG_PAGING
|
||||
|
||||
case TSTATE_WAIT_PAGEFILL:
|
||||
return "w:pgf";
|
||||
#endif
|
||||
|
||||
default:
|
||||
return "ERROR";
|
||||
}
|
||||
}
|
||||
|
||||
void print_load(uint64_t t, int fd, struct print_load_s *print_state)
|
||||
{
|
||||
print_state->new_time = t;
|
||||
|
||||
int i;
|
||||
uint64_t curr_time_us;
|
||||
uint64_t idle_time_us;
|
||||
char *clear_line = "";
|
||||
|
||||
/* print system information */
|
||||
if (fd == 1) {
|
||||
dprintf(fd, "\033[H"); /* move cursor home and clear screen */
|
||||
clear_line = CL;
|
||||
}
|
||||
|
||||
curr_time_us = t;
|
||||
idle_time_us = system_load.tasks[0].total_runtime;
|
||||
|
||||
if (print_state->new_time > print_state->interval_start_time) {
|
||||
print_state->interval_time_ms_inv = 1.f / ((float)((print_state->new_time - print_state->interval_start_time) / 1000));
|
||||
}
|
||||
|
||||
print_state->running_count = 0;
|
||||
print_state->blocked_count = 0;
|
||||
print_state->total_user_time = 0;
|
||||
|
||||
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
|
||||
uint64_t interval_runtime;
|
||||
|
||||
if (system_load.tasks[i].valid) {
|
||||
switch (system_load.tasks[i].tcb->task_state) {
|
||||
case TSTATE_TASK_PENDING:
|
||||
case TSTATE_TASK_READYTORUN:
|
||||
case TSTATE_TASK_RUNNING:
|
||||
print_state->running_count++;
|
||||
break;
|
||||
|
||||
case TSTATE_TASK_INVALID:
|
||||
case TSTATE_TASK_INACTIVE:
|
||||
case TSTATE_WAIT_SEM:
|
||||
#ifndef CONFIG_DISABLE_SIGNALS
|
||||
case TSTATE_WAIT_SIG:
|
||||
#endif
|
||||
#ifndef CONFIG_DISABLE_MQUEUE
|
||||
case TSTATE_WAIT_MQNOTEMPTY:
|
||||
case TSTATE_WAIT_MQNOTFULL:
|
||||
#endif
|
||||
#ifdef CONFIG_PAGING
|
||||
case TSTATE_WAIT_PAGEFILL:
|
||||
#endif
|
||||
print_state->blocked_count++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
interval_runtime = (system_load.tasks[i].valid && print_state->last_times[i] > 0 &&
|
||||
system_load.tasks[i].total_runtime > print_state->last_times[i])
|
||||
? (system_load.tasks[i].total_runtime - print_state->last_times[i]) / 1000
|
||||
: 0;
|
||||
|
||||
print_state->last_times[i] = system_load.tasks[i].total_runtime;
|
||||
|
||||
if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) {
|
||||
print_state->curr_loads[i] = interval_runtime * print_state->interval_time_ms_inv;
|
||||
|
||||
if (i > 0) {
|
||||
print_state->total_user_time += interval_runtime;
|
||||
}
|
||||
|
||||
} else {
|
||||
print_state->curr_loads[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
|
||||
if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) {
|
||||
if (system_load.tasks[i].tcb->pid == 0) {
|
||||
float idle;
|
||||
float task_load;
|
||||
float sched_load;
|
||||
|
||||
idle = print_state->curr_loads[0];
|
||||
task_load = (float)(print_state->total_user_time) * print_state->interval_time_ms_inv;
|
||||
|
||||
/* this can happen if one tasks total runtime was not computed
|
||||
correctly by the scheduler instrumentation TODO */
|
||||
if (task_load > (1.f - idle)) {
|
||||
task_load = (1.f - idle);
|
||||
}
|
||||
|
||||
sched_load = 1.f - idle - task_load;
|
||||
|
||||
dprintf(fd, "%sProcesses: %d total, %d running, %d sleeping\n",
|
||||
clear_line,
|
||||
system_load.total_count,
|
||||
print_state->running_count,
|
||||
print_state->blocked_count);
|
||||
dprintf(fd, "%sCPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n",
|
||||
clear_line,
|
||||
(double)(task_load * 100.f),
|
||||
(double)(sched_load * 100.f),
|
||||
(double)(idle * 100.f));
|
||||
dprintf(fd, "%sUptime: %.3fs total, %.3fs idle\n%s\n",
|
||||
clear_line,
|
||||
(double)curr_time_us / 1000000.d,
|
||||
(double)idle_time_us / 1000000.d,
|
||||
clear_line);
|
||||
|
||||
/* header for task list */
|
||||
dprintf(fd, "%s%4s %*-s %8s %6s %11s %10s %-6s\n",
|
||||
clear_line,
|
||||
"PID",
|
||||
CONFIG_TASK_NAME_SIZE, "COMMAND",
|
||||
"CPU(ms)",
|
||||
"CPU(%)",
|
||||
"USED/STACK",
|
||||
"PRIO(BASE)",
|
||||
#if CONFIG_RR_INTERVAL > 0
|
||||
"TSLICE"
|
||||
#else
|
||||
"STATE"
|
||||
#endif
|
||||
);
|
||||
}
|
||||
|
||||
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
|
||||
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
|
||||
unsigned stack_free = 0;
|
||||
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
|
||||
|
||||
while (stack_free < stack_size) {
|
||||
if (*stack_sweeper++ != 0xff) {
|
||||
break;
|
||||
}
|
||||
|
||||
stack_free++;
|
||||
}
|
||||
|
||||
dprintf(fd, "%s%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ",
|
||||
clear_line,
|
||||
system_load.tasks[i].tcb->pid,
|
||||
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
|
||||
(system_load.tasks[i].total_runtime / 1000),
|
||||
(int)(print_state->curr_loads[i] * 100.0f),
|
||||
(int)((print_state->curr_loads[i] * 100.0f - (int)(print_state->curr_loads[i] * 100.0f)) * 1000),
|
||||
stack_size - stack_free,
|
||||
stack_size,
|
||||
system_load.tasks[i].tcb->sched_priority,
|
||||
system_load.tasks[i].tcb->base_priority);
|
||||
|
||||
#if CONFIG_RR_INTERVAL > 0
|
||||
/* print scheduling info with RR time slice */
|
||||
dprintf(fd, " %6d\n", system_load.tasks[i].tcb->timeslice);
|
||||
#else
|
||||
// print task state instead
|
||||
dprintf(fd, " %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
print_state->interval_start_time = print_state->new_time;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,65 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file printload.h
|
||||
*
|
||||
* Print the current system load.
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
struct print_load_s {
|
||||
uint64_t total_user_time;
|
||||
|
||||
int running_count;
|
||||
int blocked_count;
|
||||
|
||||
uint64_t new_time;
|
||||
uint64_t interval_start_time;
|
||||
uint64_t last_times[CONFIG_MAX_TASKS];
|
||||
float curr_loads[CONFIG_MAX_TASKS];
|
||||
float interval_time_ms_inv;
|
||||
};
|
||||
|
||||
__EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s);
|
||||
|
||||
__EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state);
|
||||
|
||||
__END_DECLS
|
||||
@@ -46,9 +46,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
|
||||
_sub_air_pressure_data(node),
|
||||
_sub_air_temperature_data(node),
|
||||
_reports(nullptr)
|
||||
{
|
||||
last_temperature = 0.0f;
|
||||
}
|
||||
{ }
|
||||
|
||||
int UavcanBarometerBridge::init()
|
||||
{
|
||||
@@ -154,7 +152,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
void UavcanBarometerBridge::air_temperature_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
|
||||
{
|
||||
last_temperature = msg.static_temperature;
|
||||
last_temperature_kelvin = msg.static_temperature;
|
||||
}
|
||||
|
||||
void UavcanBarometerBridge::air_pressure_sub_cb(const
|
||||
@@ -163,7 +161,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
|
||||
baro_report report;
|
||||
|
||||
report.timestamp = msg.getMonotonicTimestamp().toUSec();
|
||||
report.temperature = last_temperature;
|
||||
report.temperature = last_temperature_kelvin - 273.15F;
|
||||
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
|
||||
report.error_count = 0;
|
||||
|
||||
|
||||
@@ -77,6 +77,6 @@ private:
|
||||
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
|
||||
unsigned _msl_pressure = 101325;
|
||||
RingBuffer *_reports;
|
||||
float last_temperature;
|
||||
float last_temperature_kelvin = 0.0;
|
||||
|
||||
};
|
||||
|
||||
@@ -45,7 +45,7 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
|
||||
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
|
||||
_sub_mag(node)
|
||||
{
|
||||
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
|
||||
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why?
|
||||
|
||||
_scale.x_scale = 1.0F;
|
||||
_scale.y_scale = 1.0F;
|
||||
@@ -141,16 +141,16 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>
|
||||
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
|
||||
&msg)
|
||||
{
|
||||
lock();
|
||||
_report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
|
||||
_report.timestamp = msg.getMonotonicTimestamp().toUSec();
|
||||
|
||||
_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
|
||||
_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
|
||||
_report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
|
||||
_report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale;
|
||||
_report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale;
|
||||
_report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale;
|
||||
unlock();
|
||||
|
||||
publish(msg.getSrcNodeID().get(), &_report);
|
||||
|
||||
@@ -40,7 +40,7 @@
|
||||
#include "sensor_bridge.hpp"
|
||||
#include <drivers/drv_mag.h>
|
||||
|
||||
#include <uavcan/equipment/ahrs/Magnetometer.hpp>
|
||||
#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
|
||||
|
||||
class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
|
||||
{
|
||||
@@ -57,14 +57,14 @@ private:
|
||||
ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
|
||||
|
||||
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
|
||||
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &msg);
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
|
||||
void (UavcanMagnetometerBridge::*)
|
||||
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &) >
|
||||
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &) >
|
||||
MagCbBinder;
|
||||
|
||||
uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
|
||||
uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength, MagCbBinder> _sub_mag;
|
||||
mag_scale _scale = {};
|
||||
mag_report _report = {};
|
||||
};
|
||||
|
||||
@@ -41,10 +41,13 @@
|
||||
/**
|
||||
* Enable UAVCAN.
|
||||
*
|
||||
* Enables support for UAVCAN-interfaced actuators and sensors.
|
||||
* Allowed values:
|
||||
* 0 - UAVCAN disabled.
|
||||
* 1 - Enabled support for UAVCAN actuators and sensors.
|
||||
* 2 - Enabled support for dynamic node ID allocation and firmware update.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @max 2
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);
|
||||
|
||||
+19
-194
@@ -1,7 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (c) 2012, 2013, 2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -36,8 +35,8 @@
|
||||
* @file top.c
|
||||
* Tool similar to UNIX top command
|
||||
* @see http://en.wikipedia.org/wiki/Top_unix
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
@@ -49,208 +48,34 @@
|
||||
#include <poll.h>
|
||||
|
||||
#include <systemlib/cpuload.h>
|
||||
#include <systemlib/printload.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#define CL "\033[K" // clear line
|
||||
|
||||
/**
|
||||
* Start the top application.
|
||||
*/
|
||||
__EXPORT int top_main(void);
|
||||
|
||||
extern struct system_load_s system_load;
|
||||
|
||||
static const char *
|
||||
tstate_name(const tstate_t s)
|
||||
{
|
||||
switch (s) {
|
||||
case TSTATE_TASK_INVALID: return "init";
|
||||
|
||||
case TSTATE_TASK_PENDING: return "PEND";
|
||||
case TSTATE_TASK_READYTORUN: return "READY";
|
||||
case TSTATE_TASK_RUNNING: return "RUN";
|
||||
|
||||
case TSTATE_TASK_INACTIVE: return "inact";
|
||||
case TSTATE_WAIT_SEM: return "w:sem";
|
||||
#ifndef CONFIG_DISABLE_SIGNALS
|
||||
case TSTATE_WAIT_SIG: return "w:sig";
|
||||
#endif
|
||||
#ifndef CONFIG_DISABLE_MQUEUE
|
||||
case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe";
|
||||
case TSTATE_WAIT_MQNOTFULL: return "w:mqf";
|
||||
#endif
|
||||
#ifdef CONFIG_PAGING
|
||||
case TSTATE_WAIT_PAGEFILL: return "w:pgf";
|
||||
#endif
|
||||
|
||||
default:
|
||||
return "ERROR";
|
||||
}
|
||||
}
|
||||
__EXPORT int top_main(int argc, char *argv[]);
|
||||
|
||||
int
|
||||
top_main(void)
|
||||
top_main(int argc, char *argv[])
|
||||
{
|
||||
uint64_t total_user_time = 0;
|
||||
hrt_abstime curr_time = hrt_absolute_time();
|
||||
|
||||
int running_count = 0;
|
||||
int blocked_count = 0;
|
||||
|
||||
uint64_t new_time = hrt_absolute_time();
|
||||
uint64_t interval_start_time = new_time;
|
||||
|
||||
uint64_t last_times[CONFIG_MAX_TASKS];
|
||||
float curr_loads[CONFIG_MAX_TASKS];
|
||||
|
||||
for (int t = 0; t < CONFIG_MAX_TASKS; t++)
|
||||
last_times[t] = 0;
|
||||
|
||||
float interval_time_ms_inv = 0.f;
|
||||
struct print_load_s load;
|
||||
init_print_load_s(curr_time, &load);
|
||||
|
||||
/* clear screen */
|
||||
printf("\033[2J");
|
||||
dprintf(1, "\033[2J\n");
|
||||
|
||||
if (argc > 1 && !strcmp(argv[1], "once")) {
|
||||
print_load(curr_time, 1, &load);
|
||||
sleep(1);
|
||||
print_load(hrt_absolute_time(), 1, &load);
|
||||
return 0;
|
||||
}
|
||||
|
||||
for (;;) {
|
||||
int i;
|
||||
uint64_t curr_time_us;
|
||||
uint64_t idle_time_us;
|
||||
|
||||
curr_time_us = hrt_absolute_time();
|
||||
idle_time_us = system_load.tasks[0].total_runtime;
|
||||
|
||||
if (new_time > interval_start_time)
|
||||
interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000));
|
||||
|
||||
running_count = 0;
|
||||
blocked_count = 0;
|
||||
total_user_time = 0;
|
||||
|
||||
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
|
||||
uint64_t interval_runtime;
|
||||
|
||||
if (system_load.tasks[i].valid) {
|
||||
switch (system_load.tasks[i].tcb->task_state) {
|
||||
case TSTATE_TASK_PENDING:
|
||||
case TSTATE_TASK_READYTORUN:
|
||||
case TSTATE_TASK_RUNNING:
|
||||
running_count++;
|
||||
break;
|
||||
|
||||
case TSTATE_TASK_INVALID:
|
||||
case TSTATE_TASK_INACTIVE:
|
||||
case TSTATE_WAIT_SEM:
|
||||
#ifndef CONFIG_DISABLE_SIGNALS
|
||||
case TSTATE_WAIT_SIG:
|
||||
#endif
|
||||
#ifndef CONFIG_DISABLE_MQUEUE
|
||||
case TSTATE_WAIT_MQNOTEMPTY:
|
||||
case TSTATE_WAIT_MQNOTFULL:
|
||||
#endif
|
||||
#ifdef CONFIG_PAGING
|
||||
case TSTATE_WAIT_PAGEFILL:
|
||||
#endif
|
||||
blocked_count++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 &&
|
||||
system_load.tasks[i].total_runtime > last_times[i])
|
||||
? (system_load.tasks[i].total_runtime - last_times[i]) / 1000
|
||||
: 0;
|
||||
|
||||
last_times[i] = system_load.tasks[i].total_runtime;
|
||||
|
||||
if (system_load.tasks[i].valid && (new_time > interval_start_time)) {
|
||||
curr_loads[i] = interval_runtime * interval_time_ms_inv;
|
||||
|
||||
if (i > 0)
|
||||
total_user_time += interval_runtime;
|
||||
} else
|
||||
curr_loads[i] = 0;
|
||||
}
|
||||
|
||||
for (i = 0; i < CONFIG_MAX_TASKS; i++) {
|
||||
if (system_load.tasks[i].valid && (new_time > interval_start_time)) {
|
||||
if (system_load.tasks[i].tcb->pid == 0) {
|
||||
float idle;
|
||||
float task_load;
|
||||
float sched_load;
|
||||
|
||||
idle = curr_loads[0];
|
||||
task_load = (float)(total_user_time) * interval_time_ms_inv;
|
||||
|
||||
/* this can happen if one tasks total runtime was not computed
|
||||
correctly by the scheduler instrumentation TODO */
|
||||
if (task_load > (1.f - idle))
|
||||
task_load = (1.f - idle);
|
||||
|
||||
sched_load = 1.f - idle - task_load;
|
||||
|
||||
/* print system information */
|
||||
printf("\033[H"); /* move cursor home and clear screen */
|
||||
printf(CL "Processes: %d total, %d running, %d sleeping\n",
|
||||
system_load.total_count,
|
||||
running_count,
|
||||
blocked_count);
|
||||
printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n",
|
||||
(double)(task_load * 100.f),
|
||||
(double)(sched_load * 100.f),
|
||||
(double)(idle * 100.f));
|
||||
printf(CL "Uptime: %.3fs total, %.3fs idle\n\n",
|
||||
(double)curr_time_us / 1000000.d,
|
||||
(double)idle_time_us / 1000000.d);
|
||||
|
||||
/* header for task list */
|
||||
printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n",
|
||||
"PID",
|
||||
CONFIG_TASK_NAME_SIZE, "COMMAND",
|
||||
"CPU(ms)",
|
||||
"CPU(%)",
|
||||
"USED/STACK",
|
||||
"PRIO(BASE)",
|
||||
#if CONFIG_RR_INTERVAL > 0
|
||||
"TSLICE"
|
||||
#else
|
||||
"STATE"
|
||||
#endif
|
||||
);
|
||||
}
|
||||
|
||||
unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
|
||||
(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
|
||||
unsigned stack_free = 0;
|
||||
uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
|
||||
|
||||
while (stack_free < stack_size) {
|
||||
if (*stack_sweeper++ != 0xff)
|
||||
break;
|
||||
|
||||
stack_free++;
|
||||
}
|
||||
|
||||
printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ",
|
||||
system_load.tasks[i].tcb->pid,
|
||||
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
|
||||
(system_load.tasks[i].total_runtime / 1000),
|
||||
(int)(curr_loads[i] * 100.0f),
|
||||
(int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000),
|
||||
stack_size - stack_free,
|
||||
stack_size,
|
||||
system_load.tasks[i].tcb->sched_priority,
|
||||
system_load.tasks[i].tcb->base_priority);
|
||||
|
||||
#if CONFIG_RR_INTERVAL > 0
|
||||
/* print scheduling info with RR time slice */
|
||||
printf(" %6d\n", system_load.tasks[i].tcb->timeslice);
|
||||
#else
|
||||
// print task state instead
|
||||
printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
interval_start_time = new_time;
|
||||
print_load(curr_time, 1, &load);
|
||||
|
||||
/* Sleep 200 ms waiting for user input five times ~ 1s */
|
||||
for (int k = 0; k < 5; k++) {
|
||||
@@ -279,7 +104,7 @@ top_main(void)
|
||||
usleep(200000);
|
||||
}
|
||||
|
||||
new_time = hrt_absolute_time();
|
||||
curr_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
return OK;
|
||||
|
||||
Reference in New Issue
Block a user