mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Merge branch 'master' into rc_timeout
This commit is contained in:
Anton Babushkin
commit
aa9ce7cd24
@ -87,6 +87,7 @@
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
#include <uORB/topics/system_power.h>
|
||||
|
||||
#include <drivers/airspeed/airspeed.h>
|
||||
|
||||
@ -121,6 +122,14 @@ protected:
|
||||
virtual int collect();
|
||||
|
||||
math::LowPassFilter2p _filter;
|
||||
|
||||
/**
|
||||
* Correct for 5V rail voltage variations
|
||||
*/
|
||||
void voltage_correction(float &diff_pres_pa, float &temperature);
|
||||
|
||||
int _t_system_power;
|
||||
struct system_power_s system_power;
|
||||
};
|
||||
|
||||
/*
|
||||
@ -129,10 +138,11 @@ protected:
|
||||
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
|
||||
|
||||
MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
|
||||
CONVERSION_INTERVAL, path),
|
||||
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ)
|
||||
CONVERSION_INTERVAL, path),
|
||||
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
|
||||
_t_system_power(-1)
|
||||
{
|
||||
|
||||
memset(&system_power, 0, sizeof(system_power));
|
||||
}
|
||||
|
||||
int
|
||||
@ -194,19 +204,48 @@ MEASAirspeed::collect()
|
||||
dp_raw = 0x3FFF & dp_raw;
|
||||
dT_raw = (val[2] << 8) + val[3];
|
||||
dT_raw = (0xFFE0 & dT_raw) >> 5;
|
||||
float temperature = ((200 * dT_raw) / 2047) - 50;
|
||||
float temperature = ((200.0f * dT_raw) / 2047) - 50;
|
||||
|
||||
/* calculate differential pressure. As its centered around 8000
|
||||
* and can go positive or negative, enforce absolute value
|
||||
*/
|
||||
// Calculate differential pressure. As its centered around 8000
|
||||
// and can go positive or negative
|
||||
const float P_min = -1.0f;
|
||||
const float P_max = 1.0f;
|
||||
float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
|
||||
const float PSI_to_Pa = 6894.757f;
|
||||
/*
|
||||
this equation is an inversion of the equation in the
|
||||
pressure transfer function figure on page 4 of the datasheet
|
||||
|
||||
if (diff_press_pa < 0.0f) {
|
||||
diff_press_pa = 0.0f;
|
||||
}
|
||||
We negate the result so that positive differential pressures
|
||||
are generated when the bottom port is used as the static
|
||||
port on the pitot and top port is used as the dynamic port
|
||||
*/
|
||||
float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
|
||||
float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
|
||||
|
||||
// correct for 5V rail voltage if possible
|
||||
voltage_correction(diff_press_pa_raw, temperature);
|
||||
|
||||
float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset);
|
||||
|
||||
/*
|
||||
note that we return both the absolute value with offset
|
||||
applied and a raw value without the offset applied. This
|
||||
makes it possible for higher level code to detect if the
|
||||
user has the tubes connected backwards, and also makes it
|
||||
possible to correctly use offsets calculated by a higher
|
||||
level airspeed driver.
|
||||
|
||||
With the above calculation the MS4525 sensor will produce a
|
||||
positive number when the top port is used as a dynamic port
|
||||
and bottom port is used as the static port
|
||||
|
||||
Also note that the _diff_pres_offset is applied before the
|
||||
fabsf() not afterwards. It needs to be done this way to
|
||||
prevent a bias at low speeds, but this also means that when
|
||||
setting a offset you must set it based on the raw value, not
|
||||
the offset value
|
||||
*/
|
||||
|
||||
struct differential_pressure_s report;
|
||||
|
||||
/* track maximum differential pressure measured (so we can work out top speed). */
|
||||
@ -219,6 +258,13 @@ MEASAirspeed::collect()
|
||||
report.temperature = temperature;
|
||||
report.differential_pressure_pa = diff_press_pa;
|
||||
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
|
||||
|
||||
/* the dynamics of the filter can make it overshoot into the negative range */
|
||||
if (report.differential_pressure_filtered_pa < 0.0f) {
|
||||
report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
|
||||
}
|
||||
|
||||
report.differential_pressure_raw_pa = diff_press_pa_raw;
|
||||
report.voltage = 0;
|
||||
report.max_differential_pressure_pa = _max_differential_pressure_pa;
|
||||
|
||||
@ -287,6 +333,62 @@ MEASAirspeed::cycle()
|
||||
USEC2TICK(CONVERSION_INTERVAL));
|
||||
}
|
||||
|
||||
/**
|
||||
correct for 5V rail voltage if the system_power ORB topic is
|
||||
available
|
||||
|
||||
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
|
||||
offset versus voltage for 3 sensors
|
||||
*/
|
||||
void
|
||||
MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
|
||||
{
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
if (_t_system_power == -1) {
|
||||
_t_system_power = orb_subscribe(ORB_ID(system_power));
|
||||
}
|
||||
if (_t_system_power == -1) {
|
||||
// not available
|
||||
return;
|
||||
}
|
||||
bool updated = false;
|
||||
orb_check(_t_system_power, &updated);
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
|
||||
}
|
||||
if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
|
||||
// not valid, skip correction
|
||||
return;
|
||||
}
|
||||
|
||||
const float slope = 65.0f;
|
||||
/*
|
||||
apply a piecewise linear correction, flattening at 0.5V from 5V
|
||||
*/
|
||||
float voltage_diff = system_power.voltage5V_v - 5.0f;
|
||||
if (voltage_diff > 0.5f) {
|
||||
voltage_diff = 0.5f;
|
||||
}
|
||||
if (voltage_diff < -0.5f) {
|
||||
voltage_diff = -0.5f;
|
||||
}
|
||||
diff_press_pa -= voltage_diff * slope;
|
||||
|
||||
/*
|
||||
the temperature masurement varies as well
|
||||
*/
|
||||
const float temp_slope = 0.887f;
|
||||
voltage_diff = system_power.voltage5V_v - 5.0f;
|
||||
if (voltage_diff > 0.5f) {
|
||||
voltage_diff = 0.5f;
|
||||
}
|
||||
if (voltage_diff < -1.0f) {
|
||||
voltage_diff = -1.0f;
|
||||
}
|
||||
temperature -= voltage_diff * temp_slope;
|
||||
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
@ -409,7 +511,7 @@ test()
|
||||
}
|
||||
|
||||
warnx("single read");
|
||||
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
|
||||
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
|
||||
|
||||
/* start the sensor polling at 2Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
|
||||
@ -437,7 +539,7 @@ test()
|
||||
}
|
||||
|
||||
warnx("periodic read %u", i);
|
||||
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
|
||||
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
|
||||
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
|
||||
}
|
||||
|
||||
|
||||
@ -952,6 +952,7 @@ PX4IO::task_main()
|
||||
param_t failsafe_param = param_find("RC_FAILS_THR");
|
||||
|
||||
if (failsafe_param > 0) {
|
||||
|
||||
param_get(failsafe_param, &failsafe_param_val);
|
||||
uint16_t failsafe_thr = failsafe_param_val;
|
||||
pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
|
||||
|
||||
@ -124,6 +124,8 @@ private:
|
||||
|
||||
orb_advert_t _range_finder_topic;
|
||||
|
||||
unsigned _consecutive_fail_count;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
perf_counter_t _buffer_overflows;
|
||||
@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) :
|
||||
_linebuf_index(0),
|
||||
_last_read(0),
|
||||
_range_finder_topic(-1),
|
||||
_consecutive_fail_count(0),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
|
||||
@ -720,12 +723,17 @@ SF0X::cycle()
|
||||
if (OK != collect_ret) {
|
||||
|
||||
/* we know the sensor needs about four seconds to initialize */
|
||||
if (hrt_absolute_time() > 5 * 1000 * 1000LL) {
|
||||
log("collection error");
|
||||
if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
|
||||
log("collection error #%u", _consecutive_fail_count);
|
||||
}
|
||||
_consecutive_fail_count++;
|
||||
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
} else {
|
||||
/* apparently success */
|
||||
_consecutive_fail_count = 0;
|
||||
}
|
||||
|
||||
/* next phase is measurement */
|
||||
|
||||
@ -41,6 +41,7 @@
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <board_config.h>
|
||||
#include <drivers/device/device.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
@ -64,6 +65,8 @@
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
#include <uORB/topics/system_power.h>
|
||||
|
||||
/*
|
||||
* Register accessors.
|
||||
* For now, no reason not to just use ADC1.
|
||||
@ -119,6 +122,8 @@ private:
|
||||
unsigned _channel_count;
|
||||
adc_msg_s *_samples; /**< sample buffer */
|
||||
|
||||
orb_advert_t _to_system_power;
|
||||
|
||||
/** work trampoline */
|
||||
static void _tick_trampoline(void *arg);
|
||||
|
||||
@ -134,13 +139,16 @@ private:
|
||||
*/
|
||||
uint16_t _sample(unsigned channel);
|
||||
|
||||
// update system_power ORB topic, only on FMUv2
|
||||
void update_system_power(void);
|
||||
};
|
||||
|
||||
ADC::ADC(uint32_t channels) :
|
||||
CDev("adc", ADC_DEVICE_PATH),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
|
||||
_channel_count(0),
|
||||
_samples(nullptr)
|
||||
_samples(nullptr),
|
||||
_to_system_power(0)
|
||||
{
|
||||
_debug_enabled = true;
|
||||
|
||||
@ -290,6 +298,43 @@ ADC::_tick()
|
||||
/* scan the channel set and sample each */
|
||||
for (unsigned i = 0; i < _channel_count; i++)
|
||||
_samples[i].am_data = _sample(_samples[i].am_channel);
|
||||
update_system_power();
|
||||
}
|
||||
|
||||
void
|
||||
ADC::update_system_power(void)
|
||||
{
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
system_power_s system_power;
|
||||
system_power.timestamp = hrt_absolute_time();
|
||||
|
||||
system_power.voltage5V_v = 0;
|
||||
for (unsigned i = 0; i < _channel_count; i++) {
|
||||
if (_samples[i].am_channel == 4) {
|
||||
// it is 2:1 scaled
|
||||
system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
|
||||
}
|
||||
}
|
||||
|
||||
// these are not ADC related, but it is convenient to
|
||||
// publish these to the same topic
|
||||
system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS);
|
||||
|
||||
// note that the valid pins are active low
|
||||
system_power.brick_valid = !stm32_gpioread(GPIO_VDD_BRICK_VALID);
|
||||
system_power.servo_valid = !stm32_gpioread(GPIO_VDD_SERVO_VALID);
|
||||
|
||||
// OC pins are active low
|
||||
system_power.periph_5V_OC = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC);
|
||||
system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC);
|
||||
|
||||
/* lazily publish */
|
||||
if (_to_system_power > 0) {
|
||||
orb_publish(ORB_ID(system_power), _to_system_power, &system_power);
|
||||
} else {
|
||||
_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
|
||||
}
|
||||
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
}
|
||||
|
||||
uint16_t
|
||||
|
||||
@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample)
|
||||
// do the filtering
|
||||
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
|
||||
if (isnan(delay_element_0) || isinf(delay_element_0)) {
|
||||
// don't allow bad values to propogate via the filter
|
||||
// don't allow bad values to propagate via the filter
|
||||
delay_element_0 = sample;
|
||||
}
|
||||
float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
|
||||
@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample)
|
||||
return output;
|
||||
}
|
||||
|
||||
float LowPassFilter2p::reset(float sample) {
|
||||
_delay_element_1 = _delay_element_2 = sample;
|
||||
return apply(sample);
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
||||
|
||||
@ -52,18 +52,30 @@ public:
|
||||
_delay_element_1 = _delay_element_2 = 0;
|
||||
}
|
||||
|
||||
// change parameters
|
||||
/**
|
||||
* Change filter parameters
|
||||
*/
|
||||
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
|
||||
|
||||
// apply - Add a new raw value to the filter
|
||||
// and retrieve the filtered result
|
||||
/**
|
||||
* Add a new raw value to the filter
|
||||
*
|
||||
* @return retrieve the filtered result
|
||||
*/
|
||||
float apply(float sample);
|
||||
|
||||
// return the cutoff frequency
|
||||
/**
|
||||
* Return the cutoff frequency
|
||||
*/
|
||||
float get_cutoff_freq(void) const {
|
||||
return _cutoff_freq;
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the filter state to this value
|
||||
*/
|
||||
float reset(float sample);
|
||||
|
||||
private:
|
||||
float _cutoff_freq;
|
||||
float _a1;
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013, 2014 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
|
||||
@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||
{
|
||||
/* give directions */
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
mavlink_log_info(mavlink_fd, "don't move system");
|
||||
mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
|
||||
|
||||
const int calibration_count = 2500;
|
||||
const int calibration_count = 2000;
|
||||
|
||||
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
struct differential_pressure_s diff_pres;
|
||||
@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||
if (fd > 0) {
|
||||
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
|
||||
paramreset_successful = true;
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
|
||||
}
|
||||
close(fd);
|
||||
}
|
||||
|
||||
if (!paramreset_successful) {
|
||||
warn("WARNING: failed to set scale / offsets for airspeed sensor");
|
||||
mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
|
||||
warn("FAILED to set scale / offsets for airspeed");
|
||||
mavlink_log_critical(mavlink_fd, "dpress reset failed");
|
||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
return ERROR;
|
||||
}
|
||||
@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)
|
||||
|
||||
if (poll_ret) {
|
||||
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
|
||||
diff_pres_offset += diff_pres.differential_pressure_pa;
|
||||
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
|
||||
calibration_counter++;
|
||||
|
||||
if (calibration_counter % (calibration_count / 20) == 0)
|
||||
|
||||
@ -1503,7 +1503,7 @@ Navigator::check_mission_item_reached()
|
||||
/* check yaw if defined only for rotary wing except takeoff */
|
||||
float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
|
||||
|
||||
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
|
||||
if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
|
||||
_waypoint_yaw_reached = true;
|
||||
}
|
||||
|
||||
|
||||
@ -84,6 +84,8 @@
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/system_power.h>
|
||||
#include <uORB/topics/servorail_status.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
@ -796,6 +798,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct telemetry_status_s telemetry;
|
||||
struct range_finder_report range_finder;
|
||||
struct estimator_status_report estimator_status;
|
||||
struct system_power_s system_power;
|
||||
struct servorail_status_s servorail_status;
|
||||
} buf;
|
||||
|
||||
memset(&buf, 0, sizeof(buf));
|
||||
@ -828,6 +832,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct log_DIST_s log_DIST;
|
||||
struct log_TELE_s log_TELE;
|
||||
struct log_ESTM_s log_ESTM;
|
||||
struct log_PWR_s log_PWR;
|
||||
} body;
|
||||
} log_msg = {
|
||||
LOG_PACKET_HEADER_INIT(0)
|
||||
@ -859,6 +864,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
int telemetry_sub;
|
||||
int range_finder_sub;
|
||||
int estimator_status_sub;
|
||||
int system_power_sub;
|
||||
int servorail_status_sub;
|
||||
} subs;
|
||||
|
||||
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
@ -884,6 +891,8 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
|
||||
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
|
||||
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
|
||||
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
|
||||
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
|
||||
|
||||
thread_running = true;
|
||||
|
||||
@ -1185,6 +1194,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.msg_type = LOG_AIRS_MSG;
|
||||
log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
|
||||
log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
|
||||
log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
|
||||
LOGBUFFER_WRITE_AND_COUNT(AIRS);
|
||||
}
|
||||
|
||||
@ -1227,6 +1237,24 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
LOGBUFFER_WRITE_AND_COUNT(BATT);
|
||||
}
|
||||
|
||||
/* --- SYSTEM POWER RAILS --- */
|
||||
if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
|
||||
log_msg.msg_type = LOG_PWR_MSG;
|
||||
log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
|
||||
log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
|
||||
log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
|
||||
log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
|
||||
log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
|
||||
log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
|
||||
|
||||
/* copy servo rail status topic here too */
|
||||
orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
|
||||
log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
|
||||
log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
|
||||
|
||||
LOGBUFFER_WRITE_AND_COUNT(PWR);
|
||||
}
|
||||
|
||||
/* --- TELEMETRY --- */
|
||||
if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
|
||||
log_msg.msg_type = LOG_TELE_MSG;
|
||||
|
||||
@ -175,6 +175,7 @@ struct log_OUT0_s {
|
||||
struct log_AIRS_s {
|
||||
float indicated_airspeed;
|
||||
float true_airspeed;
|
||||
float air_temperature_celsius;
|
||||
};
|
||||
|
||||
/* --- ARSP - ATTITUDE RATE SET POINT --- */
|
||||
@ -278,6 +279,29 @@ struct log_TELE_s {
|
||||
uint8_t txbuf;
|
||||
};
|
||||
|
||||
/* --- ESTM - ESTIMATOR STATUS --- */
|
||||
#define LOG_ESTM_MSG 23
|
||||
struct log_ESTM_s {
|
||||
float s[10];
|
||||
uint8_t n_states;
|
||||
uint8_t states_nan;
|
||||
uint8_t covariance_nan;
|
||||
uint8_t kalman_gain_nan;
|
||||
};
|
||||
|
||||
/* --- PWR - ONBOARD POWER SYSTEM --- */
|
||||
#define LOG_PWR_MSG 24
|
||||
struct log_PWR_s {
|
||||
float peripherals_5v;
|
||||
float servo_rail_5v;
|
||||
float servo_rssi;
|
||||
uint8_t usb_ok;
|
||||
uint8_t brick_ok;
|
||||
uint8_t servo_ok;
|
||||
uint8_t low_power_rail_overcurrent;
|
||||
uint8_t high_power_rail_overcurrent;
|
||||
};
|
||||
|
||||
/********** SYSTEM MESSAGES, ID > 0x80 **********/
|
||||
|
||||
/* --- TIME - TIME STAMP --- */
|
||||
@ -300,23 +324,6 @@ struct log_PARM_s {
|
||||
float value;
|
||||
};
|
||||
|
||||
/* --- ESTM - ESTIMATOR STATUS --- */
|
||||
#define LOG_ESTM_MSG 132
|
||||
struct log_ESTM_s {
|
||||
float s[10];
|
||||
uint8_t n_states;
|
||||
uint8_t states_nan;
|
||||
uint8_t covariance_nan;
|
||||
uint8_t kalman_gain_nan;
|
||||
};
|
||||
// struct log_ESTM_s {
|
||||
// float s[32];
|
||||
// uint8_t n_states;
|
||||
// uint8_t states_nan;
|
||||
// uint8_t covariance_nan;
|
||||
// uint8_t kalman_gain_nan;
|
||||
// };
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
/* construct list of all message formats */
|
||||
@ -333,7 +340,7 @@ static const struct log_format_s log_formats[] = {
|
||||
LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
|
||||
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
|
||||
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
|
||||
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
|
||||
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
|
||||
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
|
||||
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
|
||||
LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
|
||||
@ -343,16 +350,16 @@ static const struct log_format_s log_formats[] = {
|
||||
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
|
||||
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
|
||||
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
|
||||
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
|
||||
LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
|
||||
|
||||
/* system-level messages, ID >= 0x80 */
|
||||
// FMT: don't write format of format message, it's useless
|
||||
/* FMT: don't write format of format message, it's useless */
|
||||
LOG_FORMAT(TIME, "Q", "StartTime"),
|
||||
LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
|
||||
LOG_FORMAT(PARM, "Nf", "Name,Value"),
|
||||
LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
|
||||
//LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
|
||||
LOG_FORMAT(PARM, "Nf", "Name,Value")
|
||||
};
|
||||
|
||||
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
|
||||
static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);
|
||||
|
||||
#endif /* SDLOG2_MESSAGES_H_ */
|
||||
|
||||
@ -1013,12 +1013,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
|
||||
raw.differential_pressure_timestamp = _diff_pres.timestamp;
|
||||
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
|
||||
|
||||
float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
|
||||
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
|
||||
|
||||
_airspeed.timestamp = _diff_pres.timestamp;
|
||||
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
|
||||
_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
|
||||
raw.baro_pres_mbar * 1e2f, air_temperature_celcius);
|
||||
raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
|
||||
_airspeed.air_temperature_celsius = air_temperature_celsius;
|
||||
|
||||
/* announce the airspeed if needed, just publish else */
|
||||
if (_airspeed_pub > 0) {
|
||||
|
||||
@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s);
|
||||
#include "topics/servorail_status.h"
|
||||
ORB_DEFINE(servorail_status, struct servorail_status_s);
|
||||
|
||||
#include "topics/system_power.h"
|
||||
ORB_DEFINE(system_power, struct system_power_s);
|
||||
|
||||
#include "topics/vehicle_global_position.h"
|
||||
ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
|
||||
|
||||
|
||||
@ -52,9 +52,10 @@
|
||||
* Airspeed
|
||||
*/
|
||||
struct airspeed_s {
|
||||
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
|
||||
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
|
||||
float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */
|
||||
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
|
||||
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
|
||||
float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@ -52,13 +52,14 @@
|
||||
* Differential pressure.
|
||||
*/
|
||||
struct differential_pressure_s {
|
||||
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
|
||||
uint64_t error_count;
|
||||
uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */
|
||||
uint64_t error_count; /**< Number of errors detected by driver */
|
||||
float differential_pressure_pa; /**< Differential pressure reading */
|
||||
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
|
||||
float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */
|
||||
float max_differential_pressure_pa; /**< Maximum differential pressure reading */
|
||||
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
|
||||
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
|
||||
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
|
||||
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
|
||||
|
||||
};
|
||||
|
||||
|
||||
71
src/modules/uORB/topics/system_power.h
Normal file
71
src/modules/uORB/topics/system_power.h
Normal file
@ -0,0 +1,71 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 system_power.h
|
||||
*
|
||||
* Definition of the system_power voltage and power status uORB topic.
|
||||
*/
|
||||
|
||||
#ifndef SYSTEM_POWER_H_
|
||||
#define SYSTEM_POWER_H_
|
||||
|
||||
#include "../uORB.h"
|
||||
#include <stdint.h>
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* voltage and power supply status
|
||||
*/
|
||||
struct system_power_s {
|
||||
uint64_t timestamp; /**< microseconds since system boot */
|
||||
float voltage5V_v; /**< peripheral 5V rail voltage */
|
||||
uint8_t usb_connected:1; /**< USB is connected when 1 */
|
||||
uint8_t brick_valid:1; /**< brick power is good when 1 */
|
||||
uint8_t servo_valid:1; /**< servo power is good when 1 */
|
||||
uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */
|
||||
uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(system_power);
|
||||
|
||||
#endif
|
||||
Loading…
x
Reference in New Issue
Block a user