Compare commits

...

18 Commits

Author SHA1 Message Date
Beat Küng b076cfd4ed uorb: do not open a node exclusively for an advertiser
Exclusive open is not required, but we now need to ensure the queue size
is set atomically.

It avoids a race condition between 2 single-instance advertisers,
where one of them would fail to open the node with -EBUSY.
2019-12-03 14:30:25 -05:00
Beat Küng 6de6235fa9 uorb: fix several race conditions during topic initialization
Possible race conditions (they all happen between the check of existence
of a topic and trying to create the node):
- single instance, with multiple advertisers during the first advertise:
  both advertisers see the topic as non-existent and try to advertise it.
  One of them will fail, leading to an error message.
  This is the cause for telemetry_status advert failure seen in SITL in
  rare cases.
- multi-instance: subscription to non-existing instance -> px4_open fails,
  and the subscriber tries to create the node. If during that time a
  publisher publishes that instance, the subscriber will get (instance+1)
  (or fails if the max number of instances is exceeded).
  This is a race that goes pretty much unnoticed.
- multi-instance: 2 publishers can get the same instance (if is_published()
  is false in case both have not published data yet).
  This can also go unnoticed.
  Therefore the patch changes where _advertised is set: it is now set
  directly during the advertisement instead of during publication.
2019-12-03 14:30:25 -05:00
Beat Küng c5ec557a38 refactor uorb: rename published to advertised
No semantic change (yet)
2019-12-03 14:30:25 -05:00
Fabian Schilling 106905871d Use bc to support floating point PX4_SIM_SPEED_FACTOR 2019-07-03 13:59:06 -04:00
Beat Küng c92c90d4d9
logger: handle 'char' type in messages
Fixes errors like:
ERROR [logger] No definition for topic char[10] key;uint8_t[2] _padding0; found
when the Debug logging profile is selected.
2019-07-03 10:28:21 -04:00
dlwalter 7ab48dae57
px4_fmu-v5: rc.board_sensors start lis3mdl optional external magnetometer 2019-07-01 14:53:01 -04:00
RomanBapst 1fe70b2d6a
ekf2: fixed calculation of static pressure error
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2019-06-27 12:38:05 -04:00
Mathieu Bresciani 220f5cc565
mc_pos_control: Explicitly convert tilt variables to radians during check and assignments 2019-06-27 12:38:00 -04:00
Lorenz Meier 4a1d16a06b
MAVLink module: Add additional IMUs as default outputs
This simplifies setup and verification of systems, as all three sensors can be looked at in a graph view.
2019-06-26 10:25:00 -04:00
Angel 92e9228fcd
Missed conversion to radians in AutoMapper and AutoMapper2 2019-06-26 09:23:55 -04:00
Matthias Grob f8db8650d0
Testing: hotfix to recover test coverage CI
CMAKE_TESTING should automatically be enabled
but I hoped to do that in the test.cmake
target specific options and not in the main
CMakeLists. I have to see if I can make that
order work. Here the hotfix to make CI work
again.
2019-06-24 16:56:12 -04:00
Matthias Grob 415b366c54
mc_att_control: Increase default rate integral gain
@bkueang and me realized that on every frame we tune the integral gain for
the roll and pitch rate controller is much too low. Usually it needs to be
increased to 0.3 or even 0.4 to have better "locked in" flight performance
and 0.2 seems like a good compromise for a safe default.
2019-06-24 12:36:34 -04:00
Matthias Grob 40e804d393
Airframes: rename normal S500 to generic and remove PX4 defaults 2019-06-24 12:36:31 -04:00
Matthias Grob 7195ae9ebb
Airframes: add Holybro S500 Kit which was tested at dev summit 2019-06-24 12:36:26 -04:00
Beat Küng 6956d85e10
mc rate controller: add I term reduction factor
Reduce the I gain for high rate errors to reduce bounce-back effects after
flips. Up to 200 degrees the gain is almost not reduced (<25%), so this
will only take noticeable effects for large errors (setpoint changes),
where we actually want to have an effect.

This allows to increase the MC_*RATE_I parameters w/o negative effects
when doing flips (i.e. bounce-back after flips).

The 400 degrees limit and the x^2 are empirical.

The better the rate tracking in general (high P gain), the less this is
required (because of the lower tracking error). At the same time it also
does not harm, as the i_factor will always be close to 1.
2019-06-24 11:56:57 -04:00
David Sidrane 011f4990ff Add CUAV 5+ and Nano to fmu-v5 manifest
* rcS: Set SYS_USE_IO for Nano
2019-06-22 18:30:54 +02:00
Martina Rivizzigno 413acc57be
reset the position lock only if current triplet latitude and longitude
are valid
2019-06-12 14:21:16 -04:00
Matthias Grob 0c110a4443
vtol_att_control: apply multicopter takeoff hotfix also for vtol (#12250)
Please see reference:
https://github.com/PX4/Firmware/issues/12171
2019-06-12 14:16:06 -04:00
27 changed files with 144 additions and 96 deletions

View File

@ -415,6 +415,9 @@ endif()
# optionally enable cmake testing (supported only on posix)
option(CMAKE_TESTING "Configure test targets" OFF)
if (${PX4_CONFIG} STREQUAL "px4_sitl_test")
set(CMAKE_TESTING ON)
endif()
if(CMAKE_TESTING)
include(CTest) # sets BUILD_TESTING variable
endif()

View File

@ -344,7 +344,6 @@ format:
.PHONY: rostest python_coverage
tests:
$(eval CMAKE_ARGS += -DCMAKE_TESTING=ON)
$(eval CMAKE_ARGS += -DCONFIG=px4_sitl_test)
$(eval CMAKE_ARGS += -DTESTFILTER=$(TESTFILTER))
$(eval ARGS += test_results)

View File

@ -162,15 +162,15 @@ fi
# Adapt timeout parameters if simulation runs faster or slower than realtime.
if [ ! -z $PX4_SIM_SPEED_FACTOR ]; then
COM_DL_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR * 10))
COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER"
param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER
COM_RC_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR * 1))
COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1" | bc)
echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER"
param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER
COM_OF_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR * 10))
COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER"
param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER
fi

View File

@ -1,6 +1,6 @@
#!/bin/sh
#
# @name S500
# @name S500 Generic
#
# @type Quadrotor x
# @class Copter
@ -15,12 +15,10 @@ set PWM_OUT 1234
if [ $AUTOCNF = yes ]
then
param set MC_ROLL_P 6.5
param set MC_ROLLRATE_P 0.18
param set MC_ROLLRATE_I 0.15
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 6.5
param set MC_PITCHRATE_P 0.18
param set MC_ROLLRATE_I 0.15
param set MC_PITCHRATE_I 0.15
param set MC_ROLLRATE_D 0.003
param set MC_PITCHRATE_D 0.003
fi

View File

@ -0,0 +1,26 @@
#!/bin/sh
#
# @name Holybro S500
#
# @type Quadrotor x
# @class Copter
#
# @maintainer Lorenz Meier <lorenz@px4.io>
#
sh /etc/init.d/rc.mc_defaults
set MIXER quad_x
set PWM_OUT 1234
if [ $AUTOCNF = yes ]
then
param set IMU_GYRO_CUTOFF 80
param set MC_DTERM_CUTOFF 40
param set MC_ROLLRATE_P 0.14
param set MC_PITCHRATE_P 0.14
param set MC_ROLLRATE_I 0.3
param set MC_PITCHRATE_I 0.3
param set MC_ROLLRATE_D 0.004
param set MC_PITCHRATE_D 0.004
fi

View File

@ -69,6 +69,7 @@ px4_add_romfs_files(
4012_quad_x_can
4013_bebop
4014_s500
4015_holybro_s500
4020_hk_micro_pcb
4030_3dr_solo
4031_3dr_quad

View File

@ -259,7 +259,7 @@ else
then
# Check for the mini using build with px4io fw file
# but not a px4IO
if ver hwtypecmp V540
if ver hwtypecmp V540 V560
then
param set SYS_USE_IO 0
else

View File

@ -20,6 +20,7 @@ ist8310 -C -b 1 start
ist8310 -C -b 2 start
hmc5883 -C -T -X start
qmc5883 -X start
lis3mdl -X start
# Possible internal compass
ist8310 -C -b 5 start

View File

@ -90,7 +90,9 @@ static const px4_hw_mft_item_t hw_mft_list_v0540[] = {
static px4_hw_mft_list_entry_t mft_lists[] = {
{0x0000, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)},
{0x0105, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5 R:5 V:1
{0x0500, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5+ R:0 V:5
{0x0400, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)},
{0x0600, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)}, // Alias for CUAV V5nano R:0 V:6
};

View File

@ -73,7 +73,7 @@
/** Get the minimum interval at which the topic can be seen to be updated for this subscription */
#define ORBIOCGETINTERVAL _ORBIOC(16)
/** Check whether the topic is published, sets *(unsigned long *)arg to 1 if published, 0 otherwise */
#define ORBIOCISPUBLISHED _ORBIOC(17)
/** Check whether the topic is advertised, sets *(unsigned long *)arg to 1 if advertised, 0 otherwise */
#define ORBIOCISADVERTISED _ORBIOC(17)
#endif /* _DRV_UORB_H */

View File

@ -178,10 +178,12 @@ bool FlightTaskAuto::_evaluateTriplets()
} else {
tmp_target(0) = _lock_position_xy(0);
tmp_target(1) = _lock_position_xy(1);
_lock_position_xy.setAll(NAN);
}
} else {
// reset locked position if current lon and lat are valid
_lock_position_xy.setAll(NAN);
// Convert from global to local frame.
map_projection_project(&_reference_position,
_sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &tmp_target(0), &tmp_target(1));

View File

@ -122,7 +122,7 @@ void FlightTaskAutoMapper::_generateLandSetpoints()
_position_setpoint = Vector3f(_target(0), _target(1), NAN);
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _param_mpc_land_speed.get()));
// set constraints
_constraints.tilt = _param_mpc_tiltmax_lnd.get();
_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
_constraints.speed_down = _param_mpc_land_speed.get();
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}

View File

@ -132,7 +132,7 @@ void FlightTaskAutoMapper2::_prepareLandSetpoints()
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, speed_lnd));
// set constraints
_constraints.tilt = _param_mpc_tiltmax_lnd.get();
_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}

View File

@ -966,8 +966,8 @@ void Ekf2::run()
const float y_v2 = fminf(vel_body_wind(1) * vel_body_wind(1), max_airspeed_sq);
const float z_v2 = fminf(vel_body_wind(2) * vel_body_wind(2), max_airspeed_sq);
const float pstatic_err = 0.5f * airdata.rho *
(K_pstatic_coef_x * x_v2) + (K_pstatic_coef_y * y_v2) + (_param_ekf2_pcoef_z.get() * z_v2);
const float pstatic_err = 0.5f * airdata.rho * (
K_pstatic_coef_x * x_v2 + K_pstatic_coef_y * y_v2 + _param_ekf2_pcoef_z.get() * z_v2);
// correct baro measurement using pressure error estimate and assuming sea level gravity
balt_data_avg += pstatic_err / (airdata.rho * CONSTANTS_ONE_G);

View File

@ -1798,7 +1798,8 @@ void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats
strcmp(type_name, "uint64_t") != 0 &&
strcmp(type_name, "float") != 0 &&
strcmp(type_name, "double") != 0 &&
strcmp(type_name, "bool") != 0) {
strcmp(type_name, "bool") != 0 &&
strcmp(type_name, "char") != 0) {
// find orb meta for type
const orb_metadata *const*topics = orb_get_topics();

View File

@ -1704,7 +1704,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f);
configure_stream_local("RC_CHANNELS", 20.0f);
configure_stream_local("SCALED_IMU", 50.0f);
configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f);
configure_stream_local("SYS_STATUS", 5.0f);
configure_stream_local("SYSTEM_TIME", 1.0f);
@ -1792,6 +1791,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("PING", 1.0f);
configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
configure_stream_local("RC_CHANNELS", 10.0f);
configure_stream_local("SCALED_IMU", 25.0f);
configure_stream_local("SCALED_IMU2", 25.0f);
configure_stream_local("SCALED_IMU3", 25.0f);
configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f);
configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f);
configure_stream_local("SYS_STATUS", 1.0f);

View File

@ -521,7 +521,7 @@ MulticopterAttitudeControl::control_attitude()
{
vehicle_attitude_setpoint_poll();
// reinitialize the setpoint while not armed to make sure no value from the last flight is still kept
// reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept
if (!_v_control_mode.flag_armed) {
Quatf().copyTo(_v_att_sp.q_d);
Vector3f().copyTo(_v_att_sp.thrust_body);
@ -644,8 +644,17 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
}
// I term factor: reduce the I gain with increasing rate error.
// This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint
// change (noticeable in a bounce-back effect after a flip).
// The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:
// with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),
// and up to 200 deg error leads to <25% reduction of I.
float i_factor = rates_err(i) / math::radians(400.f);
i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
// Perform the integration using a first order method and do not propagate the result if out of range or invalid
float rate_i = _rates_int(i) + rates_i_scaled(i) * rates_err(i) * dt;
float rate_i = _rates_int(i) + i_factor * rates_i_scaled(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -_rate_int_lim(i) && rate_i < _rate_int_lim(i)) {
_rates_int(i) = rate_i;

View File

@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f);
* @increment 0.01
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.05f);
PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.2f);
/**
* Roll rate integrator limit
@ -151,7 +151,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f);
* @increment 0.01
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.05f);
PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.2f);
/**
* Pitch rate integrator limit

View File

@ -331,9 +331,11 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints
// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
// constraints, then just use global constraints for the limits.
const float tilt_max_radians = math::radians(math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()));
if (!PX4_ISFINITE(constraints.tilt)
|| !(constraints.tilt < math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()))) {
_constraints.tilt = math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get());
|| !(constraints.tilt < tilt_max_radians)) {
_constraints.tilt = tilt_max_radians;
}
if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _param_mpc_z_vel_max_up.get())) {
@ -352,8 +354,4 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints
void PositionControl::updateParams()
{
ModuleParams::updateParams();
// Tilt needs to be in radians
_param_mpc_tiltmax_air.set(math::radians(_param_mpc_tiltmax_air.get()));
_param_mpc_man_tilt_max.set(math::radians(_param_mpc_man_tilt_max.get()));
}

View File

@ -232,9 +232,9 @@ private:
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>)
_param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in radians
_param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in degrees
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>)
_param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in radians
_param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in degrees
(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
(ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p,
(ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,

View File

@ -55,7 +55,7 @@ uORB::DeviceMaster::~DeviceMaster()
}
int
uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, int priority)
uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority)
{
int ret = PX4_ERROR;
@ -119,17 +119,36 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in
delete node;
if (ret == -EEXIST) {
/* if the node exists already, get the existing one and check if
* something has been published yet. */
/* if the node exists already, get the existing one and check if it's advertised. */
uORB::DeviceNode *existing_node = getDeviceNodeLocked(meta, group_tries);
if ((existing_node != nullptr) && !(existing_node->is_published())) {
/* nothing has been published yet, lets claim it */
/*
* We can claim an existing node in these cases:
* - The node is not advertised (yet). It means there is already one or more subscribers or it was
* unadvertised.
* - We are a single-instance advertiser requesting the first instance.
* (Usually we don't end up here, but we might in case of a race condition between 2
* advertisers).
* - We are a subscriber requesting a certain instance.
* (Also we usually don't end up in that case, but we might in case of a race condtion
* between an advertiser and subscriber).
*/
bool is_single_instance_advertiser = is_advertiser && !instance;
if (existing_node != nullptr &&
(!existing_node->is_advertised() || is_single_instance_advertiser || !is_advertiser)) {
if (is_advertiser) {
existing_node->set_priority(priority);
/* Set as advertised to avoid race conditions (otherwise 2 multi-instance advertisers
* could get the same instance).
*/
existing_node->mark_as_advertised();
}
ret = PX4_OK;
} else {
/* otherwise: data has already been published, keep looking */
/* otherwise: already advertised, keep looking */
}
}
@ -137,7 +156,11 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in
free((void *)devpath);
} else {
// add to the node map;.
if (is_advertiser) {
node->mark_as_advertised();
}
// add to the node map.
_node_list.add(node);
}

View File

@ -60,7 +60,7 @@ class uORB::DeviceMaster
{
public:
int advertise(const struct orb_metadata *meta, int *instance, int priority);
int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority);
/**
* Public interface for getDeviceNodeLocked(). Takes care of synchronization.

View File

@ -73,35 +73,15 @@ uORB::DeviceNode::~DeviceNode()
int
uORB::DeviceNode::open(cdev::file_t *filp)
{
int ret;
/* is this a publisher? */
if (filp->f_oflags == PX4_F_WRONLY) {
/* become the publisher if we can */
lock();
if (_publisher == 0) {
_publisher = px4_getpid();
ret = PX4_OK;
} else {
ret = -EBUSY;
}
mark_as_advertised();
unlock();
/* now complete the open */
if (ret == PX4_OK) {
ret = CDev::open(filp);
/* open failed - not the publisher anymore */
if (ret != PX4_OK) {
_publisher = 0;
}
}
return ret;
return CDev::open(filp);
}
/* is this a new subscriber? */
@ -119,7 +99,7 @@ uORB::DeviceNode::open(cdev::file_t *filp)
filp->f_priv = (void *)sd;
ret = CDev::open(filp);
int ret = CDev::open(filp);
add_internal_subscriber();
@ -142,11 +122,7 @@ uORB::DeviceNode::open(cdev::file_t *filp)
int
uORB::DeviceNode::close(cdev::file_t *filp)
{
/* is this the publisher closing? */
if (px4_getpid() == _publisher) {
_publisher = 0;
} else {
if (filp->f_oflags == PX4_F_RDONLY) { /* subscriber */
SubscriberData *sd = filp_to_sd(filp);
if (sd != nullptr) {
@ -275,7 +251,6 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
_generation++;
_published = true;
ATOMIC_LEAVE;
@ -353,10 +328,12 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
*(int *)arg = get_priority();
return PX4_OK;
case ORBIOCSETQUEUESIZE:
//no need for locking here, since this is used only during the advertisement call,
//and only one advertiser is allowed to open the DeviceNode at the same time.
return update_queue_size(arg);
case ORBIOCSETQUEUESIZE: {
lock();
int ret = update_queue_size(arg);
unlock();
return ret;
}
case ORBIOCGETINTERVAL:
if (sd->update_interval) {
@ -368,8 +345,8 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
return OK;
case ORBIOCISPUBLISHED:
*(unsigned long *)arg = _published;
case ORBIOCISADVERTISED:
*(unsigned long *)arg = _advertised;
return OK;
@ -447,7 +424,7 @@ int uORB::DeviceNode::unadvertise(orb_advert_t handle)
* of subscribers and publishers. But we also do not have a leak since future
* publishers reuse the same DeviceNode object.
*/
devnode->_published = false;
devnode->_advertised = false;
return PX4_OK;
}

View File

@ -156,11 +156,13 @@ public:
void remove_internal_subscriber();
/**
* Return true if this topic has been published.
* Return true if this topic has been advertised.
*
* This is used in the case of multi_pub/sub to check if it's valid to advertise
* and publish to this node or if another node should be tried. */
bool is_published() const { return _published; }
bool is_advertised() const { return _advertised; }
void mark_as_advertised() { _advertised = true; }
/**
* Try to change the size of the queue. This can only be done as long as nobody published yet.
@ -228,13 +230,10 @@ private:
hrt_abstime _last_update{0}; /**< time the object was last updated */
volatile unsigned _generation{0}; /**< object generation count */
uint8_t _priority; /**< priority of the topic */
bool _published{false}; /**< has ever data been published */
bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */
uint8_t _queue_size; /**< maximum number of elements in the queue */
int8_t _subscriber_count{0};
px4_task_t _publisher{0}; /**< if nonzero, current publisher. Only used inside the advertise call.
We allow one publisher to have an open file descriptor at the same time. */
// statistics
uint32_t _lost_messages = 0; /**< nr of lost messages for all subscribers. If two subscribers lose the same
message, it is counted as two. */

View File

@ -105,7 +105,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
uORB::DeviceNode *node = _device_master->getDeviceNode(meta, instance);
if (node != nullptr) {
if (node->is_published()) {
if (node->is_advertised()) {
return PX4_OK;
}
}
@ -139,10 +139,10 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
int fd = px4_open(path, 0);
if (fd >= 0) {
unsigned long is_published;
unsigned long is_advertised;
if (px4_ioctl(fd, ORBIOCISPUBLISHED, (unsigned long)&is_published) == 0) {
if (!is_published) {
if (px4_ioctl(fd, ORBIOCISADVERTISED, (unsigned long)&is_advertised) == 0) {
if (!is_advertised) {
ret = PX4_ERROR;
}
}
@ -316,14 +316,12 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
return ret;
}
int uORB::Manager::node_advertise(const struct orb_metadata *meta, int *instance, int priority)
int uORB::Manager::node_advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority)
{
int ret = PX4_ERROR;
/* fill advertiser data */
if (get_device_master()) {
ret = _device_master->advertise(meta, instance, priority);
ret = _device_master->advertise(meta, is_advertiser, instance, priority);
}
/* it's PX4_OK if it already exists */
@ -373,7 +371,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i
if (fd < 0) {
/* try to create the node */
ret = node_advertise(meta, instance, priority);
ret = node_advertise(meta, advertiser, instance, priority);
if (ret == PX4_OK) {
/* update the path, as it might have been updated during the node_advertise call */
@ -385,7 +383,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i
}
}
/* on success, try the open again */
/* on success, try to open again */
if (ret == PX4_OK) {
fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY);
}

View File

@ -386,11 +386,9 @@ private: // class methods
/**
* Advertise a node; don't consider it an error if the node has
* already been advertised.
*
* @todo verify that the existing node is the same as the one
* we tried to advertise.
*/
int node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT);
int node_advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance = nullptr,
int priority = ORB_PRIO_DEFAULT);
/**
* Common implementation for orb_advertise and orb_subscribe.

View File

@ -48,6 +48,9 @@
*/
#include "vtol_att_control_main.h"
#include <systemlib/mavlink_log.h>
#include <matrix/matrix/math.hpp>
using namespace matrix;
namespace VTOL_att_control
{
@ -701,6 +704,14 @@ void VtolAttitudeControl::task_main()
_vtol_type->fill_actuator_outputs();
}
// reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept
if (!_v_control_mode.flag_armed) {
Quatf().copyTo(_mc_virtual_att_sp.q_d);
Vector3f().copyTo(_mc_virtual_att_sp.thrust_body);
Quatf().copyTo(_v_att_sp.q_d);
Vector3f().copyTo(_v_att_sp.thrust_body);
}
/* Only publish if the proper mode(s) are enabled */
if (_v_control_mode.flag_control_attitude_enabled ||
_v_control_mode.flag_control_rates_enabled ||