mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 21:33:34 +08:00
Merged status changes
This commit is contained in:
Lorenz Meier
+87
-34
@@ -80,6 +80,7 @@
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/servorail_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <debug.h>
|
||||
|
||||
@@ -266,6 +267,7 @@ private:
|
||||
orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
|
||||
orb_advert_t _to_outputs; ///< mixed servo outputs topic
|
||||
orb_advert_t _to_battery; ///< battery status / voltage
|
||||
orb_advert_t _to_servorail; ///< servorail status
|
||||
orb_advert_t _to_safety; ///< status of safety
|
||||
|
||||
actuator_outputs_s _outputs; ///<mixed outputs
|
||||
@@ -424,8 +426,27 @@ private:
|
||||
*/
|
||||
void dsm_bind_ioctl(int dsmMode);
|
||||
|
||||
};
|
||||
/**
|
||||
* Handle a battery update from IO.
|
||||
*
|
||||
* Publish IO battery information if necessary.
|
||||
*
|
||||
* @param vbatt vbatt register
|
||||
* @param ibatt ibatt register
|
||||
*/
|
||||
void io_handle_battery(uint16_t vbatt, uint16_t ibatt);
|
||||
|
||||
/**
|
||||
* Handle a servorail update from IO.
|
||||
*
|
||||
* Publish servo rail information if necessary.
|
||||
*
|
||||
* @param vservo vservo register
|
||||
* @param vrssi vrssi register
|
||||
*/
|
||||
void io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
|
||||
|
||||
};
|
||||
|
||||
namespace
|
||||
{
|
||||
@@ -461,6 +482,7 @@ PX4IO::PX4IO(device::Device *interface) :
|
||||
_to_actuators_effective(0),
|
||||
_to_outputs(0),
|
||||
_to_battery(0),
|
||||
_to_servorail(0),
|
||||
_to_safety(0),
|
||||
_primary_pwm_device(false),
|
||||
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
|
||||
@@ -1205,10 +1227,67 @@ PX4IO::io_handle_alarms(uint16_t alarms)
|
||||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
|
||||
{
|
||||
/* only publish if battery has a valid minimum voltage */
|
||||
if (vbatt <= 3300) {
|
||||
return;
|
||||
}
|
||||
|
||||
battery_status_s battery_status;
|
||||
battery_status.timestamp = hrt_absolute_time();
|
||||
|
||||
/* voltage is scaled to mV */
|
||||
battery_status.voltage_v = vbatt / 1000.0f;
|
||||
|
||||
/*
|
||||
ibatt contains the raw ADC count, as 12 bit ADC
|
||||
value, with full range being 3.3v
|
||||
*/
|
||||
battery_status.current_a = ibatt * (3.3f/4096.0f) * _battery_amp_per_volt;
|
||||
battery_status.current_a += _battery_amp_bias;
|
||||
|
||||
/*
|
||||
integrate battery over time to get total mAh used
|
||||
*/
|
||||
if (_battery_last_timestamp != 0) {
|
||||
_battery_mamphour_total += battery_status.current_a *
|
||||
(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
|
||||
}
|
||||
battery_status.discharged_mah = _battery_mamphour_total;
|
||||
_battery_last_timestamp = battery_status.timestamp;
|
||||
|
||||
/* lazily publish the battery voltage */
|
||||
if (_to_battery > 0) {
|
||||
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
|
||||
} else {
|
||||
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
|
||||
{
|
||||
servorail_status_s servorail_status;
|
||||
servorail_status.timestamp = hrt_absolute_time();
|
||||
|
||||
/* voltage is scaled to mV */
|
||||
servorail_status.voltage_v = vservo * 0.001f;
|
||||
servorail_status.rssi_v = vrssi * 0.001f;
|
||||
|
||||
/* lazily publish the servorail voltages */
|
||||
if (_to_servorail > 0) {
|
||||
orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
|
||||
} else {
|
||||
_to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
PX4IO::io_get_status()
|
||||
{
|
||||
uint16_t regs[4];
|
||||
uint16_t regs[6];
|
||||
int ret;
|
||||
|
||||
/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
|
||||
@@ -1218,40 +1297,14 @@ PX4IO::io_get_status()
|
||||
|
||||
io_handle_status(regs[0]);
|
||||
io_handle_alarms(regs[1]);
|
||||
|
||||
/* only publish if battery has a valid minimum voltage */
|
||||
if (regs[2] > 3300) {
|
||||
battery_status_s battery_status;
|
||||
|
||||
battery_status.timestamp = hrt_absolute_time();
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
io_handle_battery(regs[2], regs[3]);
|
||||
#endif
|
||||
|
||||
/* voltage is scaled to mV */
|
||||
battery_status.voltage_v = regs[2] / 1000.0f;
|
||||
|
||||
/*
|
||||
regs[3] contains the raw ADC count, as 12 bit ADC
|
||||
value, with full range being 3.3v
|
||||
*/
|
||||
battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt;
|
||||
battery_status.current_a += _battery_amp_bias;
|
||||
|
||||
/*
|
||||
integrate battery over time to get total mAh used
|
||||
*/
|
||||
if (_battery_last_timestamp != 0) {
|
||||
_battery_mamphour_total += battery_status.current_a *
|
||||
(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
|
||||
}
|
||||
battery_status.discharged_mah = _battery_mamphour_total;
|
||||
_battery_last_timestamp = battery_status.timestamp;
|
||||
|
||||
/* lazily publish the battery voltage */
|
||||
if (_to_battery > 0) {
|
||||
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
|
||||
} else {
|
||||
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
|
||||
}
|
||||
}
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
io_handle_vservo(regs[4], regs[5]);
|
||||
#endif
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user