forked from Archive/PX4-Autopilot
Merged status changes
This commit is contained in:
commit
90c4664dce
|
@ -690,6 +690,7 @@ BMA180::measure()
|
|||
* measurement flow without using the external interrupt.
|
||||
*/
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = 0;
|
||||
/*
|
||||
* y of board is x of sensor and x of board is -y of sensor
|
||||
* perform only the axis assignment here.
|
||||
|
|
|
@ -52,6 +52,7 @@
|
|||
*/
|
||||
struct accel_report {
|
||||
uint64_t timestamp;
|
||||
uint64_t error_count;
|
||||
float x; /**< acceleration in the NED X board axis in m/s^2 */
|
||||
float y; /**< acceleration in the NED Y board axis in m/s^2 */
|
||||
float z; /**< acceleration in the NED Z board axis in m/s^2 */
|
||||
|
|
|
@ -55,6 +55,7 @@ struct baro_report {
|
|||
float altitude;
|
||||
float temperature;
|
||||
uint64_t timestamp;
|
||||
uint64_t error_count;
|
||||
};
|
||||
|
||||
/*
|
||||
|
|
|
@ -52,6 +52,7 @@
|
|||
*/
|
||||
struct gyro_report {
|
||||
uint64_t timestamp;
|
||||
uint64_t error_count;
|
||||
float x; /**< angular velocity in the NED X board axis in rad/s */
|
||||
float y; /**< angular velocity in the NED Y board axis in rad/s */
|
||||
float z; /**< angular velocity in the NED Z board axis in rad/s */
|
||||
|
|
|
@ -54,6 +54,7 @@
|
|||
*/
|
||||
struct mag_report {
|
||||
uint64_t timestamp;
|
||||
uint64_t error_count;
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
|
|
|
@ -52,6 +52,7 @@
|
|||
*/
|
||||
struct range_finder_report {
|
||||
uint64_t timestamp;
|
||||
uint64_t error_count;
|
||||
float distance; /** in meters */
|
||||
uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */
|
||||
};
|
||||
|
|
|
@ -153,35 +153,36 @@ ETSAirspeed::collect()
|
|||
ret = transfer(nullptr, 0, &val[0], 2);
|
||||
|
||||
if (ret < 0) {
|
||||
log("error reading from sensor: %d", ret);
|
||||
perf_count(_comms_errors);
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint16_t diff_pres_pa = val[1] << 8 | val[0];
|
||||
if (diff_pres_pa == 0) {
|
||||
// a zero value means the pressure sensor cannot give us a
|
||||
// value. We need to return, and not report a value or the
|
||||
// caller could end up using this value as part of an
|
||||
// average
|
||||
log("zero value from sensor");
|
||||
return -1;
|
||||
// a zero value means the pressure sensor cannot give us a
|
||||
// value. We need to return, and not report a value or the
|
||||
// caller could end up using this value as part of an
|
||||
// average
|
||||
perf_count(_comms_errors);
|
||||
log("zero value from sensor");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
|
||||
diff_pres_pa = 0;
|
||||
|
||||
} else {
|
||||
diff_pres_pa -= _diff_pres_offset;
|
||||
}
|
||||
|
||||
// Track maximum differential pressure measured (so we can work out top speed).
|
||||
if (diff_pres_pa > _max_differential_pressure_pa) {
|
||||
_max_differential_pressure_pa = diff_pres_pa;
|
||||
_max_differential_pressure_pa = diff_pres_pa;
|
||||
}
|
||||
|
||||
// XXX we may want to smooth out the readings to remove noise.
|
||||
differential_pressure_s report;
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
report.differential_pressure_pa = diff_pres_pa;
|
||||
report.voltage = 0;
|
||||
report.max_differential_pressure_pa = _max_differential_pressure_pa;
|
||||
|
@ -209,7 +210,7 @@ ETSAirspeed::cycle()
|
|||
|
||||
/* perform collection */
|
||||
if (OK != collect()) {
|
||||
log("collection error");
|
||||
perf_count(_comms_errors);
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
|
|
|
@ -791,6 +791,7 @@ HMC5883::collect()
|
|||
|
||||
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
|
||||
new_report.timestamp = hrt_absolute_time();
|
||||
new_report.error_count = perf_event_count(_comms_errors);
|
||||
|
||||
/*
|
||||
* @note We could read the status register here, which could tell us that
|
||||
|
|
|
@ -771,6 +771,7 @@ L3GD20::measure()
|
|||
* 74 from all measurements centers them around zero.
|
||||
*/
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = 0; // not recorded
|
||||
|
||||
switch (_orientation) {
|
||||
|
||||
|
|
|
@ -1248,6 +1248,7 @@ LSM303D::measure()
|
|||
|
||||
|
||||
accel_report.timestamp = hrt_absolute_time();
|
||||
accel_report.error_count = 0; // not reported
|
||||
|
||||
accel_report.x_raw = raw_accel_report.x;
|
||||
accel_report.y_raw = raw_accel_report.y;
|
||||
|
|
|
@ -490,6 +490,7 @@ MB12XX::collect()
|
|||
|
||||
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
report.distance = si_units;
|
||||
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
|
||||
|
||||
|
|
|
@ -138,7 +138,6 @@ MEASAirspeed::measure()
|
|||
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
log("i2c::transfer returned %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
@ -161,7 +160,6 @@ MEASAirspeed::collect()
|
|||
ret = transfer(nullptr, 0, &val[0], 4);
|
||||
|
||||
if (ret < 0) {
|
||||
log("error reading from sensor: %d", ret);
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
|
@ -207,6 +205,7 @@ MEASAirspeed::collect()
|
|||
}
|
||||
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
report.temperature = temperature;
|
||||
report.differential_pressure_pa = diff_press_pa;
|
||||
report.voltage = 0;
|
||||
|
@ -235,7 +234,6 @@ MEASAirspeed::cycle()
|
|||
|
||||
/* perform collection */
|
||||
if (OK != collect()) {
|
||||
log("collection error");
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
|
|
|
@ -1186,7 +1186,7 @@ MPU6000::measure()
|
|||
* Adjust and scale results to m/s^2.
|
||||
*/
|
||||
grb.timestamp = arb.timestamp = hrt_absolute_time();
|
||||
|
||||
grb.error_count = arb.error_count = 0; // not reported
|
||||
|
||||
/*
|
||||
* 1) Scale raw value to SI units using scaling from datasheet.
|
||||
|
|
|
@ -573,6 +573,7 @@ MS5611::collect()
|
|||
struct baro_report report;
|
||||
/* this should be fairly close to the end of the conversion, so the best approximation of the time */
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
|
||||
/* read the most recent measurement - read offset/size are hardcoded in the interface */
|
||||
ret = _interface->read(0, (void *)&raw, 0);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -674,27 +674,25 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
#ifdef ADC_VSERVO
|
||||
/* PX4IO_P_STATUS_VSERVO */
|
||||
{
|
||||
/*
|
||||
* Coefficients here derived by measurement of the 5-16V
|
||||
* range on one unit:
|
||||
*
|
||||
* XXX pending measurements
|
||||
*
|
||||
* slope = xxx
|
||||
* intercept = xxx
|
||||
*
|
||||
* Intercept corrected for best results @ 5.0V.
|
||||
*/
|
||||
unsigned counts = adc_measure(ADC_VSERVO);
|
||||
if (counts != 0xffff) {
|
||||
unsigned mV = (4150 + (counts * 46)) / 10 - 200;
|
||||
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000;
|
||||
|
||||
r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
|
||||
// use 3:1 scaling on 3.3V ADC input
|
||||
unsigned mV = counts * 9900 / 4096;
|
||||
r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#ifdef ADC_RSSI
|
||||
/* PX4IO_P_STATUS_VRSSI */
|
||||
{
|
||||
unsigned counts = adc_measure(ADC_RSSI);
|
||||
if (counts != 0xffff) {
|
||||
// use 1:1 scaling on 3.3V ADC input
|
||||
unsigned mV = counts * 3300 / 4096;
|
||||
r_page_status[PX4IO_P_STATUS_VRSSI] = mV;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
/* XXX PX4IO_P_STATUS_VRSSI */
|
||||
/* XXX PX4IO_P_STATUS_PRSSI */
|
||||
|
||||
SELECT_PAGE(r_page_status);
|
||||
|
|
|
@ -321,6 +321,32 @@ perf_print_counter(perf_counter_t handle)
|
|||
}
|
||||
}
|
||||
|
||||
uint64_t
|
||||
perf_event_count(perf_counter_t handle)
|
||||
{
|
||||
if (handle == NULL)
|
||||
return 0;
|
||||
|
||||
switch (handle->type) {
|
||||
case PC_COUNT:
|
||||
return ((struct perf_ctr_count *)handle)->event_count;
|
||||
|
||||
case PC_ELAPSED: {
|
||||
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
|
||||
return pce->event_count;
|
||||
}
|
||||
|
||||
case PC_INTERVAL: {
|
||||
struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
|
||||
return pci->event_count;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
perf_print_all(void)
|
||||
{
|
||||
|
|
|
@ -135,6 +135,14 @@ __EXPORT extern void perf_print_all(void);
|
|||
*/
|
||||
__EXPORT extern void perf_reset_all(void);
|
||||
|
||||
/**
|
||||
* Return current event_count
|
||||
*
|
||||
* @param handle The counter returned from perf_alloc.
|
||||
* @return event_count
|
||||
*/
|
||||
__EXPORT extern uint64_t perf_event_count(perf_counter_t handle);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
#endif
|
||||
|
|
|
@ -87,6 +87,9 @@ ORB_DEFINE(safety, struct safety_s);
|
|||
#include "topics/battery_status.h"
|
||||
ORB_DEFINE(battery_status, struct battery_status_s);
|
||||
|
||||
#include "topics/servorail_status.h"
|
||||
ORB_DEFINE(servorail_status, struct servorail_status_s);
|
||||
|
||||
#include "topics/vehicle_global_position.h"
|
||||
ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
|
||||
|
||||
|
|
|
@ -53,6 +53,7 @@
|
|||
*/
|
||||
struct differential_pressure_s {
|
||||
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
|
||||
uint64_t error_count;
|
||||
uint16_t differential_pressure_pa; /**< Differential pressure reading */
|
||||
uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */
|
||||
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
|
||||
|
|
|
@ -0,0 +1,67 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file servorail_status.h
|
||||
*
|
||||
* Definition of the servorail status uORB topic.
|
||||
*/
|
||||
|
||||
#ifndef SERVORAIL_STATUS_H_
|
||||
#define SERVORAIL_STATUS_H_
|
||||
|
||||
#include "../uORB.h"
|
||||
#include <stdint.h>
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Servorail voltages and status
|
||||
*/
|
||||
struct servorail_status_s {
|
||||
uint64_t timestamp; /**< microseconds since system boot */
|
||||
float voltage_v; /**< Servo rail voltage in volts */
|
||||
float rssi_v; /**< RSSI pin voltage in volts */
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(servorail_status);
|
||||
|
||||
#endif
|
Loading…
Reference in New Issue