aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/meas_airspeed/meas_airspeed.cpp128
-rw-r--r--src/drivers/px4io/px4io.cpp1
-rw-r--r--src/drivers/sf0x/sf0x.cpp12
-rw-r--r--src/drivers/stm32/adc/adc.cpp47
-rw-r--r--src/lib/mathlib/math/filter/LowPassFilter2p.cpp7
-rw-r--r--src/lib/mathlib/math/filter/LowPassFilter2p.hpp20
-rw-r--r--src/modules/commander/airspeed_calibration.cpp14
-rw-r--r--src/modules/navigator/navigator_main.cpp2
-rw-r--r--src/modules/sdlog2/sdlog2.c28
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h53
-rw-r--r--src/modules/sensors/sensors.cpp5
-rw-r--r--src/modules/uORB/objects_common.cpp3
-rw-r--r--src/modules/uORB/topics/airspeed.h5
-rw-r--r--src/modules/uORB/topics/differential_pressure.h9
-rw-r--r--src/modules/uORB/topics/system_power.h71
15 files changed, 346 insertions, 59 deletions
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 2c3efdc35..1ad383ee0 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -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;
@@ -288,6 +334,62 @@ MEASAirspeed::cycle()
}
/**
+ 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.
*/
namespace meas_airspeed
@@ -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);
}
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 71aa5a0a4..e318e206a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -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);
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index 07163035b..a0cf98340 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -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 */
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 0b8a275e6..3a60d2cae 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -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
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
index 3699d9bce..6f640c9f9 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
@@ -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
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
index 208ec98d4..74cd5d78c 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
@@ -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;
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 6039d92a7..c8c7a42e7 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -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)
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index b7f26198b..1bb5d33a1 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -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;
}
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 112a4bd3e..4b2d01c71 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -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;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 08eaa208c..4b70e55c9 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -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_ */
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 5b826a919..f20c328d3 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -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) {
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 4b31cc8a4..c8a589bb7 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -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);
diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h
index a3da3758f..d2ee754cd 100644
--- a/src/modules/uORB/topics/airspeed.h
+++ b/src/modules/uORB/topics/airspeed.h
@@ -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 */
};
/**
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index ff88b04c6..01e14cda9 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -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 */
};
diff --git a/src/modules/uORB/topics/system_power.h b/src/modules/uORB/topics/system_power.h
new file mode 100644
index 000000000..7763b6004
--- /dev/null
+++ b/src/modules/uORB/topics/system_power.h
@@ -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
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-15 04:45:12 +0000