fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 12:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: move battery calculations to systemlib

Browse Source 
The commander used to consume the battery_status topic and write the
contents after some calculations into the system state. Instead, the
calculations now happen in library calls in systemlib/battery.

This moves the battery fields out of the vehicle_status message into the
battery_status topic.

This brought quite some changes in all modules that need battery
information. The current state is compiling but untested.
This commit is contained in:
Julian Oes
2016-04-11 18:01:47 +02:00
parent 699b08c9fd
commit 32c3135788
21 changed files with 661 additions and 426 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gpio_led/gpio_led.c
+26 -10
View File
@@ -51,6 +51,7 @@
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <modules/px4iofirmware/protocol.h>
@@ -60,8 +61,10 @@ struct gpio_led_s {
int gpio_fd;
bool use_io;
int pin;
struct vehicle_status_s status;
struct vehicle_status_s vehicle_status;
struct battery_status_s battery_status;
int vehicle_status_sub;
int battery_status_sub;
bool led_state;
int counter;
};
@@ -238,11 +241,17 @@ void gpio_led_start(FAR void *arg)
ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
/* initialize vehicle status structure */
memset(&priv->status, 0, sizeof(priv->status));
memset(&priv->vehicle_status, 0, sizeof(priv->vehicle_status));
/* initialize battery status structure */
memset(&priv->battery_status, 0, sizeof(priv->battery_status));
/* subscribe to vehicle status topic */
priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* subscribe to battery status topic */
priv->battery_status_sub = orb_subscribe(ORB_ID(battery_status));
/* add worker to queue */
int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
@@ -258,32 +267,39 @@ void gpio_led_cycle(FAR void *arg)
{
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
/* check for status updates*/
/* check for vehicle status updates*/
bool updated;
orb_check(priv->vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->vehicle_status);
}
/* select pattern for current status */
orb_check(priv->battery_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(battery_status), priv->battery_status_sub, &priv->battery_status);
}
/* select pattern for current vehiclestatus */
int pattern = 0;
if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {
if (priv->vehicle_status.arming_state == ARMING_STATE_ARMED_ERROR) {
pattern = 0x2A; // *_*_*_ fast blink (armed, error)
} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) {
} else if (priv->vehicle_status.arming_state == ARMING_STATE_ARMED) {
if (priv->battery_status.warning == BATTERY_WARNING_NONE
&& !priv->vehicle_status.failsafe) {
pattern = 0x3f; // ****** solid (armed)
} else {
pattern = 0x3e; // *****_ slow blink (armed, battery low or failsafe)
}
} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
} else if (priv->vehicle_status.arming_state == ARMING_STATE_STANDBY) {
pattern = 0x38; // ***___ slow blink (disarmed, ready)
} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
} else if (priv->vehicle_status.arming_state == ARMING_STATE_STANDBY_ERROR) {
pattern = 0x28; // *_*___ slow double blink (disarmed, error)
}