Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Utilize DEFINE_PARAMETERS() macro for params in mavlink_receiver.cpp/h #11274

Merged
merged 3 commits into from
Jun 17, 2019
+67 −66
Merged

Utilize DEFINE_PARAMETERS() macro for params in mavlink_receiver.cpp/h #11274

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a9072af
Move variable initialization from constructor to declarations in Mavl…
mcsauder Jan 31, 2019
1096019
Rename _param_sub to _param_update_sub and initialize. Re-define and…
mcsauder Jun 13, 2019
a0a5279
Modify the update_params() comment to better reflect what is taking p…
mcsauder Jun 14, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
86 changes: 38 additions & 48 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,19 +83,13 @@
using matrix::wrap_2pi;

MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
ModuleParams(nullptr),
_mavlink(parent),
_mavlink_ftp(parent),
_mavlink_log_handler(parent),
_mavlink_timesync(parent),
_mission_manager(parent),
_parameters_manager(parent),
_p_bat_emergen_thr(param_find("BAT_EMERGEN_THR")),
_p_bat_crit_thr(param_find("BAT_CRIT_THR")),
_p_bat_low_thr(param_find("BAT_LOW_THR")),
_p_flow_rot(param_find("SENS_FLOW_ROT")),
_p_flow_maxr(param_find("SENS_FLOW_MAXR")),
_p_flow_minhgt(param_find("SENS_FLOW_MINHGT")),
_p_flow_maxhgt(param_find("SENS_FLOW_MAXHGT"))
_parameters_manager(parent)
{
/* Make the attitude quaternion valid */
_att.q[0] = 1.0f;
Expand Down Expand Up @@ -334,14 +328,7 @@ void
MavlinkReceiver::send_flight_information()
{
mavlink_flight_information_t flight_info{};

param_t param_flight_uuid = param_find("COM_FLIGHT_UUID");

if (param_flight_uuid != PARAM_INVALID) {
int32_t flight_uuid;
param_get(param_flight_uuid, &flight_uuid);
flight_info.flight_uuid = (uint64_t)flight_uuid;
}
flight_info.flight_uuid = static_cast<uint64_t>(_param_com_flight_uuid.get());

actuator_armed_s actuator_armed{};
bool ret = _actuator_armed_sub.copy(&actuator_armed);
Expand Down Expand Up @@ -579,32 +566,30 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
mavlink_optical_flow_rad_t flow;
mavlink_msg_optical_flow_rad_decode(msg, &flow);

/* read flow sensor parameters */
int32_t flow_rot_int;
param_get(_p_flow_rot, &flow_rot_int);
const enum Rotation flow_rot = (Rotation)flow_rot_int;

struct optical_flow_s f = {};
optical_flow_s f = {};

f.timestamp = _mavlink_timesync.sync_stamp(flow.time_usec);
f.integration_timespan = flow.integration_time_us;
f.time_since_last_sonar_update = flow.time_delta_distance_us;
f.integration_timespan = flow.integration_time_us;
f.pixel_flow_x_integral = flow.integrated_x;
f.pixel_flow_y_integral = flow.integrated_y;
f.gyro_x_rate_integral = flow.integrated_xgyro;
f.gyro_y_rate_integral = flow.integrated_ygyro;
f.gyro_z_rate_integral = flow.integrated_zgyro;
f.time_since_last_sonar_update = flow.time_delta_distance_us;
f.ground_distance_m = flow.distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
f.gyro_temperature = flow.temperature;
param_get(_p_flow_maxr, &f.max_flow_rate);
param_get(_p_flow_minhgt, &f.min_ground_distance);
param_get(_p_flow_maxhgt, &f.max_ground_distance);
f.gyro_x_rate_integral = flow.integrated_xgyro;
f.gyro_y_rate_integral = flow.integrated_ygyro;
f.gyro_z_rate_integral = flow.integrated_zgyro;
f.gyro_temperature = flow.temperature;
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();

/* read flow sensor parameters */
const Rotation flow_rot = (Rotation)_param_sens_flow_rot.get();

/* rotate measurements according to parameter */
float zeroval = 0.0f;
rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval);
float zero_val = 0.0f;
rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zero_val);
rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral);

if (_flow_pub == nullptr) {
Expand All @@ -615,7 +600,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
}

/* Use distance value for distance sensor topic */
struct distance_sensor_s d = {};
distance_sensor_s d = {};

if (flow.distance > 0.0f) { // negative values signal invalid data
d.timestamp = f.timestamp;
Expand Down Expand Up @@ -1536,23 +1521,15 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
battery_status.cell_count = cell_count;
battery_status.connected = true;

// Get the battery level thresholds.
float bat_emergen_thr;
float bat_crit_thr;
float bat_low_thr;
param_get(_p_bat_emergen_thr, &bat_emergen_thr);
param_get(_p_bat_crit_thr, &bat_crit_thr);
param_get(_p_bat_low_thr, &bat_low_thr);

// Set the battery warning based on remaining charge.
// Note: Smallest values must come first in evaluation.
if (battery_status.remaining < bat_emergen_thr) {
if (battery_status.remaining < _param_bat_emergen_thr.get()) {
battery_status.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;

} else if (battery_status.remaining < bat_crit_thr) {
} else if (battery_status.remaining < _param_bat_crit_thr.get()) {
battery_status.warning = battery_status_s::BATTERY_WARNING_CRITICAL;

} else if (battery_status.remaining < bat_low_thr) {
} else if (battery_status.remaining < _param_bat_low_thr.get()) {
battery_status.warning = battery_status_s::BATTERY_WARNING_LOW;
}

Expand Down Expand Up @@ -2589,6 +2566,11 @@ MavlinkReceiver::receive_thread(void *arg)
hrt_abstime last_send_update = 0;

while (!_mavlink->_task_should_exit) {
// Check for updated parameters.
if (_param_update_sub.updated()) {
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@dagar and @davids5 , this check mirrors the Navigator::run() method while loop check for updated parameters. This should prevent the issue discussed during the dev call checking for updated params every iteration of the receive_thread() while loop.

update_params();
}

if (poll(&fds[0], 1, timeout) > 0) {
if (_mavlink->get_protocol() == SERIAL) {

Expand Down Expand Up @@ -2738,3 +2720,11 @@ MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent)

pthread_attr_destroy(&receiveloop_attr);
}

void
MavlinkReceiver::update_params()
{
parameter_update_s param_update;
_param_update_sub.update(&param_update);
updateParams();
}
47 changes: 29 additions & 18 deletions src/modules/mavlink/mavlink_receiver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,7 @@

#pragma once

#include <perf/perf_counter.h>
#include <uORB/uORB.h>
#include <px4_module_params.h>

#include <uORB/topics/airspeed.h>
#include <uORB/topics/actuator_armed.h>
Expand Down Expand Up @@ -91,7 +90,7 @@

class Mavlink;

class MavlinkReceiver
class MavlinkReceiver : public ModuleParams
{
public:
/**
Expand Down Expand Up @@ -188,11 +187,17 @@ class MavlinkReceiver

void send_storage_information(int storage_id);

/**
* @brief Updates the battery, optical flow, and flight ID subscribed parameters.
*/
void update_params();

Mavlink *_mavlink;

MavlinkFTP _mavlink_ftp;
MavlinkLogHandler _mavlink_log_handler;
MavlinkTimesync _mavlink_timesync;

MavlinkMissionManager _mission_manager;
MavlinkParametersManager _parameters_manager;

Expand Down Expand Up @@ -246,32 +251,38 @@ class MavlinkReceiver
orb_advert_t _transponder_report_pub{nullptr};
orb_advert_t _visual_odometry_pub{nullptr};

static constexpr int _gps_inject_data_queue_size{6};

uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};

static constexpr int _gps_inject_data_queue_size{6};

static constexpr unsigned int MOM_SWITCH_COUNT{8};

int _orb_class_instance{-1};

uint64_t _global_ref_timestamp{0};
uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {};

bool _hil_local_proj_inited{false};
uint16_t _mom_switch_state{0};

float _hil_local_alt0{0.0f};
uint64_t _global_ref_timestamp{0};

static constexpr unsigned MOM_SWITCH_COUNT{8};
float _hil_local_alt0{0.0f};

uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {};
uint16_t _mom_switch_state{0};
bool _hil_local_proj_inited{false};

param_t _p_bat_emergen_thr{PARAM_INVALID};
param_t _p_bat_crit_thr{PARAM_INVALID};
param_t _p_bat_low_thr{PARAM_INVALID};
param_t _p_flow_rot{PARAM_INVALID};
param_t _p_flow_maxr{PARAM_INVALID};
param_t _p_flow_minhgt{PARAM_INVALID};
param_t _p_flow_maxhgt{PARAM_INVALID};
uORB::Subscription _param_update_sub{ORB_ID(parameter_update)};

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::COM_FLIGHT_UUID>) _param_com_flight_uuid,
(ParamInt<px4::params::SENS_FLOW_ROT>) _param_sens_flow_rot
);

// Disallow copy construction and move assignment.
MavlinkReceiver(const MavlinkReceiver &) = delete;
Expand Down