From c92b8b03cd56f6e2786f633e359081696ae9fdd0 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Thu, 31 Jan 2019 08:22:20 -0700 Subject: [PATCH] Move variable initialization from constructor to declarations in MavlinkReceiver class, alphabetize var lists. Added update_params() method to ensure that parameters subscribed to are updated routinely. --- src/modules/mavlink/mavlink_receiver.cpp | 77 +++++++++++------------- src/modules/mavlink/mavlink_receiver.h | 47 +++++++++------ 2 files changed, 64 insertions(+), 60 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 75d8aaf85c..eedab6399b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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; @@ -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(_param_com_flight_uuid.get()); actuator_armed_s actuator_armed{}; bool ret = _actuator_armed_sub.copy(&actuator_armed); @@ -580,27 +567,25 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) 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; + const enum Rotation flow_rot = (Rotation)_param_sens_flow_rot.get(); struct 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(); /* rotate measurements according to parameter */ float zeroval = 0.0f; @@ -1560,23 +1545,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; } @@ -2613,6 +2590,8 @@ MavlinkReceiver::receive_thread(void *arg) hrt_abstime last_send_update = 0; while (!_mavlink->_task_should_exit) { + update_params(false); + if (poll(&fds[0], 1, timeout) > 0) { if (_mavlink->get_protocol() == SERIAL) { @@ -2762,3 +2741,17 @@ MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent) pthread_attr_destroy(&receiveloop_attr); } + +void +MavlinkReceiver::update_params(const bool force) +{ + bool updated; + parameter_update_s param_update; + + orb_check(_param_sub, &updated); + + if (updated || force) { + updateParams(); + orb_copy(ORB_ID(parameter_update), _param_sub, ¶m_update); + } +} diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 182f96debd..d52d907d21 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -41,8 +41,7 @@ #pragma once -#include -#include +#include #include #include @@ -91,7 +90,7 @@ class Mavlink; -class MavlinkReceiver +class MavlinkReceiver : public ModuleParams { public: /** @@ -188,11 +187,18 @@ private: void send_storage_information(int storage_id); + /** + * @brief Updates and checks for updated uORB parameters. + * @param force Boolean to determine if an update check should be forced. + */ + void update_params(const bool force = false); + Mavlink *_mavlink; MavlinkFTP _mavlink_ftp; MavlinkLogHandler _mavlink_log_handler; MavlinkTimesync _mavlink_timesync; + MavlinkMissionManager _mission_manager; MavlinkParametersManager _parameters_manager; @@ -246,32 +252,37 @@ private: 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}; + int _param_sub{-1}; + + uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {}; + + uint16_t _mom_switch_state{0}; uint64_t _global_ref_timestamp{0}; - bool _hil_local_proj_inited{false}; - float _hil_local_alt0{0.0f}; - static constexpr unsigned MOM_SWITCH_COUNT{8}; + bool _hil_local_proj_inited{false}; - uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {}; - uint16_t _mom_switch_state{0}; - - 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}; + DEFINE_PARAMETERS( + (ParamFloat) _param_bat_crit_thr, + (ParamFloat) _param_bat_emergen_thr, + (ParamFloat) _param_bat_low_thr, + (ParamFloat) _param_sens_flow_maxhgt, + (ParamFloat) _param_sens_flow_maxr, + (ParamFloat) _param_sens_flow_minhgt, + (ParamInt) _param_com_flight_uuid, + (ParamInt) _param_sens_flow_rot + ); // Disallow copy construction and move assignment. MavlinkReceiver(const MavlinkReceiver &) = delete;