fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 14:07:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Removed all swapping of uORB instances

Browse Source
This commit is contained in:
Timothy Scott
2020-02-07 11:48:56 +01:00
committed by Lorenz Meier
parent 6452b7e014
commit 4c1adc088f
5 changed files with 77 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/battery/battery.cpp
-8
View File
@@ -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)
{
src/lib/battery/battery.h
-9
View File
@@ -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;
src/modules/battery_status/battery_status.cpp
-14
View File
@@ -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 */
src/modules/commander/Commander.cpp
+70 -71
View File
@@ -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;
}
}
src/modules/commander/Commander.hpp
+7 -2
View File
@@ -43,7 +43,6 @@
#include <lib/mathlib/mathlib.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <containers/Array.hpp>
// publications
#include <uORB/Publication.hpp>
@@ -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)};