Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-22 19:46:53 +08:00
Code Issues Actions 34 Packages Projects Releases Wiki Activity

mavlink_receiver: updated sens flow parameters to be dynamically handle

Browse Source
This commit is contained in:
Igor Mišić
2021-09-23 09:23:22 +02:00
committed by Beat Küng
parent e73172b537
commit 9ad0e5b357
2 changed files with 46 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+31 -4
View File
@@ -88,6 +88,10 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_parameters_manager(parent),
_mavlink_timesync(parent)
{
_handle_sens_flow_maxhgt = param_find("SENS_FLOW_MAXHGT");
_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");
}
void
@@ -693,12 +697,12 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
f.max_flow_rate = _param_sens_flow_maxr.get();
f.min_ground_distance = _param_sens_flow_minhgt.get();
f.max_ground_distance = _param_sens_flow_maxhgt.get();
f.max_flow_rate = _param_sens_flow_maxr;
f.min_ground_distance = _param_sens_flow_minhgt;
f.max_ground_distance = _param_sens_flow_maxhgt;
/* read flow sensor parameters */
const Rotation flow_rot = (Rotation)_param_sens_flow_rot.get();
const Rotation flow_rot = (Rotation)_param_sens_flow_rot;
/* rotate measurements according to parameter */
float zero_val = 0.0f;
@@ -3339,6 +3343,29 @@ void MavlinkReceiver::start()
pthread_attr_destroy(&receiveloop_attr);
}
void
MavlinkReceiver::updateParams()
{
// update parameters from storage
ModuleParams::updateParams();
if (_handle_sens_flow_maxhgt != PARAM_INVALID) {
param_get(_handle_sens_flow_maxhgt, &_param_sens_flow_maxhgt);
}
if (_handle_sens_flow_maxr != PARAM_INVALID) {
param_get(_handle_sens_flow_maxr, &_param_sens_flow_maxr);
}
if (_handle_sens_flow_minhgt != PARAM_INVALID) {
param_get(_handle_sens_flow_minhgt, &_param_sens_flow_minhgt);
}
if (_handle_sens_flow_rot != PARAM_INVALID) {
param_get(_handle_sens_flow_rot, &_param_sens_flow_rot);
}
}
void *MavlinkReceiver::start_trampoline(void *context)
{
MavlinkReceiver *self = reinterpret_cast<MavlinkReceiver *>(context);
src/modules/mavlink/mavlink_receiver.h
+15 -5
View File
@@ -232,6 +232,10 @@ private:
px4::atomic_bool _should_exit{false};
pthread_t _thread {};
/**
* @brief Updates optical flow parameters.
*/
void updateParams() override;
Mavlink *_mavlink;
@@ -375,14 +379,20 @@ private:
hrt_abstime _heartbeat_component_udp_bridge{0};
hrt_abstime _heartbeat_component_uart_bridge{0};
param_t _handle_sens_flow_maxhgt{PARAM_INVALID};
param_t _handle_sens_flow_maxr{PARAM_INVALID};
param_t _handle_sens_flow_minhgt{PARAM_INVALID};
param_t _handle_sens_flow_rot{PARAM_INVALID};
float _param_sens_flow_maxhgt{-1.0f};
float _param_sens_flow_maxr{-1.0f};
float _param_sens_flow_minhgt{-1.0f};
float _param_sens_flow_rot{-1.0f};
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::SENS_FLOW_MAXHGT>) _param_sens_flow_maxhgt,
(ParamFloat<px4::params::SENS_FLOW_MAXR>) _param_sens_flow_maxr,
(ParamFloat<px4::params::SENS_FLOW_MINHGT>) _param_sens_flow_minhgt,
(ParamInt<px4::params::SENS_FLOW_ROT>) _param_sens_flow_rot
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr
);
// Disallow copy construction and move assignment.