diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index dda42a056e..db5346afab 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -165,14 +165,6 @@ Battery::publish() orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance, ORB_PRIO_DEFAULT); } -void -Battery::swapUorbAdvert(Battery &other) -{ - orb_advert_t tmp = _orb_advert; - _orb_advert = other._orb_advert; - other._orb_advert = tmp; -} - void Battery::filterVoltage(float voltage_v) { diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index ae9c7b039a..52014edacc 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -108,15 +108,6 @@ public: */ void publish(); - /** - * Some old functionality expects the primary battery to be published on instance 0. To maintain backwards - * compatibility, this function allows the advertisements (and therefore instances) of 2 batteries to be swapped. - * However, this should not be relied upon anywhere, and should be considered for all intents deprecated. - * - * The proper way to uniquely identify batteries is by the `id` field in the `battery_status` message. - */ - void swapUorbAdvert(Battery &other); - protected: struct { param_t v_empty; diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index 4960cc79f7..e54065a278 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -117,10 +117,6 @@ private: #endif }; // End _analogBatteries -#if BOARD_NUMBER_BRICKS > 1 - int _battery_pub_intance0ndx {0}; /**< track the index of instance 0 */ -#endif /* BOARD_NUMBER_BRICKS > 1 */ - perf_counter_t _loop_perf; /**< loop performance counter */ /** @@ -210,17 +206,7 @@ BatteryStatus::adc_poll() * VDD_5V_IN */ selected_source = b; -# if BOARD_NUMBER_BRICKS > 1 - /* Move the selected_source to instance 0 */ - if (_battery_pub_intance0ndx != selected_source) { - _analogBatteries[_battery_pub_intance0ndx]->swapUorbAdvert( - *_analogBatteries[selected_source] - ); - _battery_pub_intance0ndx = selected_source; - } - -# endif /* BOARD_NUMBER_BRICKS > 1 */ } /* look for specific channels and process the raw voltage to measurement data */ diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c670960287..24849ca906 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3717,7 +3717,7 @@ void Commander::battery_status_check() size_t num_connected_batteries = 0; - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + for (size_t i = 0; i < sizeof(_battery_subs) / sizeof(_battery_subs[0]); i++) { if (_battery_subs[i].updated() && _battery_subs[i].copy(&batteries[num_connected_batteries])) { // We need to update the status flag if ANY battery is updated, because the system source might have // changed, or might be nothing (if there is no battery connected) @@ -3729,84 +3729,83 @@ void Commander::battery_status_check() } } - /* update battery status */ - if (battery_sub_updated) { + if (!battery_sub_updated) { + // Nothing has changed since the last time this function was called, so nothing needs to be done now. + return; + } - // There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest - // option is to check if ANY of them have a warning, and specifically find which one has the most - // urgent warning. - uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE; - // Sum the total current of all connected batteries. This is used to detect engine failure. - float total_current = 0; - // To make sure that all connected batteries are being regularly reported, we check which one has the - // oldest timestamp. - hrt_abstime oldest_update = hrt_absolute_time(); + // There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest + // option is to check if ANY of them have a warning, and specifically find which one has the most + // urgent warning. + uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE; + // To make sure that all connected batteries are being regularly reported, we check which one has the + // oldest timestamp. + hrt_abstime oldest_update = hrt_absolute_time(); - // Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing. - for (size_t i = 0; i < num_connected_batteries; i++) { - if (batteries[i].warning > worst_warning) { - worst_warning = batteries[i].warning; - } - - if (hrt_elapsed_time(&batteries[i].timestamp) > hrt_elapsed_time(&oldest_update)) { - oldest_update = batteries[i].timestamp; - } - - total_current += batteries[i].current_filtered_a; + // Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing. + for (size_t i = 0; i < num_connected_batteries; i++) { + if (batteries[i].warning > worst_warning) { + worst_warning = batteries[i].warning; } - bool battery_warning_level_increased_while_armed = false; - bool update_internal_battery_state = false; + if (hrt_elapsed_time(&batteries[i].timestamp) > hrt_elapsed_time(&oldest_update)) { + oldest_update = batteries[i].timestamp; + } - if (armed.armed) { - if (worst_warning > _battery_warning) { - battery_warning_level_increased_while_armed = true; - update_internal_battery_state = true; - } + if (batteries[i].system_source) { + _battery_current = batteries[i].current_filtered_a; + } + } - } else { - if (_battery_warning != worst_warning) { - update_internal_battery_state = true; + bool battery_warning_level_increased_while_armed = false; + bool update_internal_battery_state = false; + + if (armed.armed) { + if (worst_warning > _battery_warning) { + battery_warning_level_increased_while_armed = true; + update_internal_battery_state = true; + } + + } else { + if (_battery_warning != worst_warning) { + update_internal_battery_state = true; + } + } + + if (update_internal_battery_state) { + _battery_warning = worst_warning; + } + + status_flags.condition_battery_healthy = + // All connected batteries are regularly being published + (hrt_elapsed_time(&oldest_update) < 5_s) + // There is at least one connected battery (in any slot) + && num_connected_batteries > 0 + // No currently-connected batteries have any warning + && (_battery_warning == battery_status_s::BATTERY_WARNING_NONE); + + // execute battery failsafe if the state has gotten worse while we are armed + if (battery_warning_level_increased_while_armed) { + battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning, + (low_battery_action_t)_param_com_low_bat_act.get()); + } + + // Handle shutdown request from emergency battery action + if (update_internal_battery_state) { + + if ((_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) && shutdown_if_allowed()) { + mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down"); + px4_usleep(200000); + + int ret_val = px4_shutdown_request(false, false); + + if (ret_val) { + mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown"); + + } else { + while (1) { px4_usleep(1); } } } - - if (update_internal_battery_state) { - _battery_warning = worst_warning; - } - - status_flags.condition_battery_healthy = - // All connected batteries are regularly being published - (hrt_elapsed_time(&oldest_update) < 5_s) - // There is at least one connected battery (in any slot) - && num_connected_batteries > 0 - // No currently-connected batteries have any warning - && (_battery_warning == battery_status_s::BATTERY_WARNING_NONE); - - // execute battery failsafe if the state has gotten worse while we are armed - if (battery_warning_level_increased_while_armed) { - battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning, - (low_battery_action_t)_param_com_low_bat_act.get()); - } - - // Handle shutdown request from emergency battery action - if (update_internal_battery_state) { - - if ((_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) && shutdown_if_allowed()) { - mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down"); - px4_usleep(200000); - - int ret_val = px4_shutdown_request(false, false); - - if (ret_val) { - mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown"); - - } else { - while (1) { px4_usleep(1); } - } - } - } - - _battery_current = total_current; } } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 684ee1ccf4..09ec70c898 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -43,7 +43,6 @@ #include #include #include -#include // publications #include @@ -375,13 +374,19 @@ private: // Subscriptions uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS}; +#if BOARD_NUMBER_BRICKS > 1 uORB::Subscription _battery_subs[ORB_MULTI_MAX_INSTANCES] { uORB::Subscription(ORB_ID(battery_status), 0), uORB::Subscription(ORB_ID(battery_status), 1), uORB::Subscription(ORB_ID(battery_status), 2), uORB::Subscription(ORB_ID(battery_status), 3), }; - uORB::Subscription _cmd_sub{ORB_ID(vehicle_command)}; +#else + uORB::Subscription _battery_subs[1] { + uORB::Subscription(ORB_ID(battery_status), 0) + }; +#endif + uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)};