From 0dc8bb9c864dc49bbbe255ea0177480b46522d7d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 24 Sep 2020 11:01:28 -0400 Subject: [PATCH] uORB: increase ORB_MULTI_MAX_INSTANCES 4 -> 10 - put more realistic bounds on maximum number of battery instances, gps, etc --- msg/battery_status.msg | 3 +++ src/modules/commander/Commander.hpp | 3 ++- src/modules/commander/mag_calibration.cpp | 2 +- src/modules/logger/logged_topics.cpp | 8 ++++---- src/modules/logger/logged_topics.h | 2 +- src/modules/mavlink/mavlink_high_latency2.cpp | 13 +++++++------ src/modules/mavlink/mavlink_high_latency2.h | 3 +-- src/modules/mavlink/mavlink_messages.cpp | 14 +++++++------- src/modules/uORB/uORB.h | 2 +- 9 files changed, 27 insertions(+), 23 deletions(-) diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 554ce1b7be..9b242d80fd 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -40,3 +40,6 @@ uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely uint8 warning # current battery warning + + +uint8 MAX_INSTANCES = 4 diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 93e8f54e6c..59953bbcac 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -397,7 +397,8 @@ private: uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; - uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; uORB::SubscriptionMultiArray _telemetry_status_subs{ORB_ID::telemetry_status}; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index cc08a1521f..4b98f4acd7 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -226,7 +226,7 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y, static float get_sphere_radius() { // if GPS is available use real field intensity from world magnetic model - uORB::SubscriptionMultiArray gps_subs{ORB_ID::vehicle_gps_position}; + uORB::SubscriptionMultiArray gps_subs{ORB_ID::vehicle_gps_position}; for (auto &gps_sub : gps_subs) { vehicle_gps_position_s gps; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 6e3bf206e9..2fd1ab79c1 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -75,7 +75,6 @@ void LoggedTopics::add_default_topics() add_topic("position_setpoint_triplet", 200); add_topic("px4io_status"); add_topic("radio_status"); - add_topic("rate_ctrl_status", 200); add_topic("rpm", 500); add_topic("safety"); add_topic("sensor_combined"); @@ -110,14 +109,15 @@ void LoggedTopics::add_default_topics() add_topic_multi("actuator_outputs", 100, 2); add_topic_multi("logger_status", 0, 2); add_topic_multi("multirotor_motor_limits", 1000, 2); + add_topic_multi("rate_ctrl_status", 200, 2); add_topic_multi("telemetry_status", 1000); add_topic_multi("wind_estimate", 1000); // log all raw sensors at minimal rate (at least 1 Hz) - add_topic_multi("battery_status", 300); - add_topic_multi("differential_pressure", 1000); + add_topic_multi("battery_status", 300, 4); + add_topic_multi("differential_pressure", 1000, 3); add_topic_multi("distance_sensor", 1000); - add_topic_multi("optical_flow", 1000); + add_topic_multi("optical_flow", 1000, 2); add_topic_multi("sensor_accel", 1000, 3); add_topic_multi("sensor_baro", 1000, 3); add_topic_multi("sensor_gyro", 1000, 3); diff --git a/src/modules/logger/logged_topics.h b/src/modules/logger/logged_topics.h index 1a15daaf38..6cf4cfcd53 100644 --- a/src/modules/logger/logged_topics.h +++ b/src/modules/logger/logged_topics.h @@ -74,7 +74,7 @@ inline bool operator&(SDLogProfileMask a, SDLogProfileMask b) class LoggedTopics { public: - static constexpr int MAX_TOPICS_NUM = 200; /**< Maximum number of logged topics */ + static constexpr int MAX_TOPICS_NUM = 255; /**< Maximum number of logged topics */ struct RequestedSubscription { uint16_t interval_ms; diff --git a/src/modules/mavlink/mavlink_high_latency2.cpp b/src/modules/mavlink/mavlink_high_latency2.cpp index 4509508df7..510d3309cb 100644 --- a/src/modules/mavlink/mavlink_high_latency2.cpp +++ b/src/modules/mavlink/mavlink_high_latency2.cpp @@ -72,7 +72,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) bool updated = _airspeed.valid(); updated |= _airspeed_sp.valid(); - for (int i = 0; i < MAX_BATTERIES; i++) { + for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { updated |= _batteries[i].analyzer.valid(); } @@ -112,7 +112,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) int lowest = 0; - for (int i = 1; i < MAX_BATTERIES; i++) { + for (int i = 1; i < battery_status_s::MAX_INSTANCES; i++) { const bool battery_connected = _batteries[i].connected && _batteries[i].analyzer.valid(); const bool battery_is_lowest = _batteries[i].analyzer.get_scaled(100.0f) <= _batteries[lowest].analyzer.get_scaled( 100.0f); @@ -183,7 +183,7 @@ void MavlinkStreamHighLatency2::reset_analysers(const hrt_abstime t) _airspeed.reset(); _airspeed_sp.reset(); - for (int i = 0; i < MAX_BATTERIES; i++) { + for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { _batteries[i].analyzer.reset(); } @@ -227,10 +227,11 @@ bool MavlinkStreamHighLatency2::write_attitude_sp(mavlink_high_latency2_t *msg) bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *msg) { - struct battery_status_s battery; bool updated = false; - for (int i = 0; i < MAX_BATTERIES; i++) { + for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { + battery_status_s battery; + if (_batteries[i].subscription.update(&battery)) { updated = true; _batteries[i].connected = battery.connected; @@ -486,7 +487,7 @@ void MavlinkStreamHighLatency2::update_battery_status() { battery_status_s battery; - for (int i = 0; i < MAX_BATTERIES; i++) { + for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { if (_batteries[i].subscription.update(&battery)) { _batteries[i].connected = battery.connected; _batteries[i].analyzer.add_value(battery.remaining, _update_rate_filtered); diff --git a/src/modules/mavlink/mavlink_high_latency2.h b/src/modules/mavlink/mavlink_high_latency2.h index 7bc125bfbf..e03e00f48f 100644 --- a/src/modules/mavlink/mavlink_high_latency2.h +++ b/src/modules/mavlink/mavlink_high_latency2.h @@ -138,8 +138,7 @@ private: hrt_abstime _last_update_time = 0; float _update_rate_filtered = 0.0f; - static constexpr int MAX_BATTERIES = 4; - PerBatteryData _batteries[MAX_BATTERIES] {0, 1, 2, 3}; + PerBatteryData _batteries[battery_status_s::MAX_INSTANCES] {0, 1, 2, 3}; /* do not allow top copying this class */ MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index adc0479149..337b86a46d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -649,7 +649,7 @@ public: private: uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; - uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &) = delete; @@ -669,7 +669,7 @@ protected: cpuload_s cpuload{}; _cpuload_sub.copy(&cpuload); - battery_status_s battery_status[ORB_MULTI_MAX_INSTANCES] {}; + battery_status_s battery_status[battery_status_s::MAX_INSTANCES] {}; for (int i = 0; i < _battery_status_subs.size(); i++) { _battery_status_subs[i].copy(&battery_status[i]); @@ -753,7 +753,7 @@ public: } private: - uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; /* do not allow top copying this class */ MavlinkStreamBatteryStatus(MavlinkStreamSysStatus &) = delete; @@ -877,7 +877,7 @@ public: } private: - uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; /* do not allow top copying this class */ MavlinkStreamSmartBatteryInfo(MavlinkStreamSysStatus &) = delete; @@ -960,7 +960,7 @@ public: } private: - uORB::SubscriptionMultiArray _vehicle_imu_subs{ORB_ID::vehicle_imu}; + uORB::SubscriptionMultiArray _vehicle_imu_subs{ORB_ID::vehicle_imu}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _bias_sub{ORB_ID(estimator_sensor_bias)}; uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; @@ -2993,7 +2993,7 @@ public: private: uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; - uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; + uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; /* do not allow top copying this class */ MavlinkStreamVibration(MavlinkStreamVibration &) = delete; @@ -4622,7 +4622,7 @@ protected: { bool updated = false; - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (int i = 0; i < _distance_sensor_subs.size(); i++) { distance_sensor_s dist_sensor; if (_distance_sensor_subs[i].update(&dist_sensor)) { diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 5e8751c5d1..5c5ea7995d 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -60,7 +60,7 @@ typedef const struct orb_metadata *orb_id_t; /** * Maximum number of multi topic instances */ -#define ORB_MULTI_MAX_INSTANCES 4 // This must be < 10 (because it's the last char of the node path) +#define ORB_MULTI_MAX_INSTANCES 10 // This must be <= 10 (because it's the last char of the node path) /** * Generates a pointer to the uORB metadata structure for