Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
CPS-VO
PX4-Firmware
Commits
b8517eb4
Commit
b8517eb4
authored
Aug 23, 2015
by
Lorenz Meier
Browse files
Merge branch 'beta' into stable
parents
630740d5
07a88e9e
Changes
27
Hide whitespace changes
Inline
Side-by-side
NuttX
@
8891d035
Compare
678eea3d
...
8891d035
Subproject commit
678eea3d2c157c686439597abb0367b0f1e643f4
Subproject commit
8891d035df45c8be570cfbd9419b438679faf7ee
ROMFS/px4fmu_common/init.d/rc.uavcan
View file @
b8517eb4
...
...
@@ -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
ROMFS/px4fmu_common/init.d/rc.usb
View file @
b8517eb4
...
...
@@ -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
ROMFS/px4fmu_common/init.d/rcS
View file @
b8517eb4
...
...
@@ -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
#
...
...
v1.0
@
c390a63a
Compare
d23a551e
...
c390a63a
Subproject commit
d23a551e108d92c7a49a66d162cb9d542bbe6be1
Subproject commit
c390a63abfce15af0629bdf207c4b6a183fed118
msg/tecs_status.msg
View file @
b8517eb4
...
...
@@ -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
src/drivers/px4fmu/fmu.cpp
View file @
b8517eb4
...
...
@@ -355,7 +355,7 @@ PX4FMU::init()
_task
=
task_spawn_cmd
(
"fmuservo"
,
SCHED_DEFAULT
,
SCHED_PRIORITY_DEFAULT
,
1
6
00
,
1
2
00
,
(
main_t
)
&
PX4FMU
::
task_main_trampoline
,
nullptr
);
...
...
src/lib/external_lgpl/tecs/tecs.cpp
View file @
b8517eb4
...
...
@@ -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.2
f
*
STEdot_error
+
0.8
f
*
_STEdotErrLast
;
_STEdotErrLast
=
STEdot_error
;
_
STEdot_error
=
0.2
f
*
_
STEdot_error
+
0.8
f
*
_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
;
...
...
src/lib/external_lgpl/tecs/tecs.h
View file @
b8517eb4
...
...
@@ -79,6 +79,10 @@ public:
_SKE_est
(
0.0
f
),
_SPEdot
(
0.0
f
),
_SKEdot
(
0.0
f
),
_STE_error
(
0.0
f
),
_STEdot_error
(
0.0
f
),
_SEB_error
(
0.0
f
),
_SEBdot_error
(
0.0
f
),
_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
;
...
...
uavcan
@
3ae5400a
Compare
46793c5e
...
3ae5400a
Subproject commit
46793c5e0699d494cb9ec10ca70b6d833acbec46
Subproject commit
3ae5400aa5ead18139106d30f730114d5e9b65dd
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View file @
b8517eb4
...
...
@@ -1115,7 +1115,7 @@ int AttitudePositionEstimatorEKF::start()
_estimator_task
=
task_spawn_cmd
(
"ekf_att_pos_estimator"
,
SCHED_DEFAULT
,
SCHED_PRIORITY_MAX
-
40
,
75
00
,
48
00
,
(
main_t
)
&
AttitudePositionEstimatorEKF
::
task_main_trampoline
,
nullptr
);
...
...
src/modules/ekf_att_pos_estimator/module.mk
View file @
b8517eb4
...
...
@@ -42,5 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \
estimator_22states.cpp
\
estimator_utilities.cpp
EXTRACXXFLAGS
=
-Weffc
++
-Wframe-larger-than
=
61
00
EXTRACXXFLAGS
=
-Weffc
++
-Wframe-larger-than
=
34
00
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View file @
b8517eb4
...
...
@@ -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
);
...
...
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View file @
b8517eb4
...
...
@@ -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 */
...
...
src/modules/mavlink/mavlink_main.cpp
View file @
b8517eb4
...
...
@@ -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.0
f
);
break
;
case
MAVLINK_MODE_CONFIG
:
// Enable a number of interesting streams we want via USB
configure_stream
(
"PARAM_VALUE"
,
300.0
f
);
configure_stream
(
"MISSION_ITEM"
,
50.0
f
);
configure_stream
(
"NAMED_VALUE_FLOAT"
,
10.0
f
);
configure_stream
(
"OPTICAL_FLOW_RAD"
,
10.0
f
);
configure_stream
(
"VFR_HUD"
,
20.0
f
);
configure_stream
(
"ATTITUDE"
,
100.0
f
);
configure_stream
(
"ACTUATOR_CONTROL_TARGET0"
,
30.0
f
);
configure_stream
(
"RC_CHANNELS_RAW"
,
5.0
f
);
configure_stream
(
"SERVO_OUTPUT_RAW_0"
,
20.0
f
);
configure_stream
(
"POSITION_TARGET_GLOBAL_INT"
,
10.0
f
);
configure_stream
(
"LOCAL_POSITION_NED"
,
30.0
f
);
configure_stream
(
"MANUAL_CONTROL"
,
5.0
f
);
configure_stream
(
"HIGHRES_IMU"
,
100.0
f
);
configure_stream
(
"GPS_RAW_INT"
,
20.0
f
);
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
;
...
...
src/modules/mavlink/mavlink_main.h
View file @
b8517eb4
...
...
@@ -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
);
...
...
src/modules/sdlog2/sdlog2.c
View file @
b8517eb4
...
...
@@ -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
,
"
\n
LOAD 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
,
"
\n
LOAD 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
);
}
...
...
src/modules/sdlog2/sdlog2_messages.h
View file @
b8517eb4
...
...
@@ -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
,
"fffffffffffff
f
B"
,
"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"
),
...
...
src/modules/systemlib/module.mk
View file @
b8517eb4
...
...
@@ -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
\
...
...
src/modules/systemlib/printload.c
0 → 100644
View file @
b8517eb4
/****************************************************************************
*
* 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
;