Browse Source

mavlink: make range params optional

If there is no range sensor compiled in, these two params do not seem to
be available. Hence, we need to make them optional.
master
Julian Oes 3 years ago committed by Daniel Agar
parent
commit
22180a2639
  1. 14
      src/modules/mavlink/mavlink_receiver.cpp
  2. 8
      src/modules/mavlink/mavlink_receiver.h

14
src/modules/mavlink/mavlink_receiver.cpp

@ -94,6 +94,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : @@ -94,6 +94,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_handle_sens_flow_maxr = param_find("SENS_FLOW_MAXR");
_handle_sens_flow_minhgt = param_find("SENS_FLOW_MINHGT");
_handle_sens_flow_rot = param_find("SENS_FLOW_ROT");
_handle_ekf2_min_rng = param_find("EKF2_MIN_RNG");
_handle_ekf2_rng_a_hmax = param_find("EKF2_RNG_A_HMAX");
}
void
@ -829,8 +831,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) @@ -829,8 +831,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
device_id.devid_s.address = msg->sysid;
d.timestamp = f.timestamp;
d.min_distance = _param_ekf2_min_rng.get();
d.max_distance = _param_ekf2_rng_a_hmax.get();
d.min_distance = _param_ekf2_min_rng;
d.max_distance = _param_ekf2_rng_a_hmax;
d.current_distance = flow.distance; /* both are in m */
d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
d.device_id = device_id.devid;
@ -3448,6 +3450,14 @@ MavlinkReceiver::updateParams() @@ -3448,6 +3450,14 @@ MavlinkReceiver::updateParams()
if (_handle_sens_flow_rot != PARAM_INVALID) {
param_get(_handle_sens_flow_rot, &_param_sens_flow_rot);
}
if (_handle_ekf2_min_rng != PARAM_INVALID) {
param_get(_handle_ekf2_min_rng, &_param_ekf2_min_rng);
}
if (_handle_ekf2_rng_a_hmax != PARAM_INVALID) {
param_get(_handle_ekf2_rng_a_hmax, &_param_ekf2_rng_a_hmax);
}
}
void *MavlinkReceiver::start_trampoline(void *context)

8
src/modules/mavlink/mavlink_receiver.h

@ -394,18 +394,20 @@ private: @@ -394,18 +394,20 @@ private:
param_t _handle_sens_flow_maxr{PARAM_INVALID};
param_t _handle_sens_flow_minhgt{PARAM_INVALID};
param_t _handle_sens_flow_rot{PARAM_INVALID};
param_t _handle_ekf2_min_rng{PARAM_INVALID};
param_t _handle_ekf2_rng_a_hmax{PARAM_INVALID};
float _param_sens_flow_maxhgt{-1.0f};
float _param_sens_flow_maxr{-1.0f};
float _param_sens_flow_minhgt{-1.0f};
int32_t _param_sens_flow_rot{0};
float _param_ekf2_min_rng{NAN};
float _param_ekf2_rng_a_hmax{NAN};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
(ParamFloat<px4::params::EKF2_MIN_RNG>) _param_ekf2_min_rng,
(ParamFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr
);
// Disallow copy construction and move assignment.

Loading…
Cancel
Save