Commit b8517eb4 authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Merge branch 'beta' into stable

parents 630740d5 07a88e9e
Subproject commit 678eea3d2c157c686439597abb0367b0f1e643f4
Subproject commit 8891d035df45c8be570cfbd9419b438679faf7ee
......@@ -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
#
......
Subproject commit d23a551e108d92c7a49a66d162cb9d542bbe6be1
Subproject commit c390a63abfce15af0629bdf207c4b6a183fed118
......@@ -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;
......
Subproject commit 46793c5e0699d494cb9ec10ca70b6d833acbec46
Subproject commit 3ae5400aa5ead18139106d30f730114d5e9b65dd
......@@ -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.flightPathAngleSp = s.dhgt_dem;
t.flightPathAngle = s.dhgt;
t.flightPathAngleFiltered = s.dhgt;
t.airspeedDerivativeSp = s.dspd_dem;
t.airspeedDerivative = s.dspd;
t.totalEnergyRateSp = s.thr;
t.totalEnergyRate = s.ithr;
t.energyDistributionRateSp = s.ptch;
t.energyDistributionRate = s.iptch;
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.altitude_rate_sp;
t.flightPathAngle = s.altitude_rate;
t.flightPathAngleFiltered = s.altitude_rate;
t.airspeedDerivativeSp = s.airspeed_rate_sp;
t.airspeedDerivative = s.airspeed_rate;
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 \
......
/****************************************************************************
*
* 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;