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

Debugging, cleanup and added airspeed to HoTT telemetry.

Browse Source
This commit is contained in:
Simon Wilks
2013-04-21 01:29:07 +02:00
parent 853ba612b1
commit 48f815860b
5 changed files with 62 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/ets_airspeed/ets_airspeed.cpp
+23 -8
View File
@@ -81,7 +81,11 @@
#define READ_CMD 0x07 /* Read the data */
/* Max measurement rate is 100Hz */
#define CONVERSION_INTERVAL (1000000 / 10) /* microseconds */
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
/* The Eagle Tree Airspeed V3 can only provide accurate readings
for speeds from 15km/h upwards. */
#define MIN_ACCURATE_DIFF_PRES_PA 12
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -222,6 +226,8 @@ ETS_AIRSPEED::init()
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct differential_pressure_s[_num_reports];
for (int i = 0; i < _num_reports; i++)
_reports[i].max_differential_pressure_pa = 0;
if (_reports == nullptr)
goto out;
@@ -235,8 +241,6 @@ ETS_AIRSPEED::init()
if (_airspeed_pub < 0)
debug("failed to create airspeed sensor object. Did you start uOrb?");
param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
@@ -454,13 +458,24 @@ ETS_AIRSPEED::collect()
uint16_t diff_pres_pa = val[1] << 8 | val[0];
/* adjust if necessary */
diff_pres_pa -= _diff_pres_offset;
//log("measurement: %0.2f m/s", calc_indicated_airspeed((float)_reports[_next_report].diff_pressure));
param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;
} else {
diff_pres_pa -= _diff_pres_offset;
}
// XXX we may want to smooth out the readings to remove noise.
_reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].differential_pressure_pa = diff_pres_pa;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
}
/* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
@@ -684,7 +699,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
warnx("diff pressure: %0.3f pa", (double) report.differential_pressure_pa);
warnx("diff pressure: %d pa", report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -709,7 +724,7 @@ test()
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("diff pressure: %0.3f pa", (double) report.differential_pressure_pa);
warnx("diff pressure: %d pa", report.differential_pressure_pa);
}
errx(0, "PASS");
apps/hott_telemetry/messages.c
+12
View File
@@ -42,9 +42,11 @@
#include <string.h>
#include <systemlib/systemlib.h>
#include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
static int airspeed_sub = -1;
static int battery_sub = -1;
static int sensor_sub = -1;
@@ -52,6 +54,7 @@ void messages_init(void)
{
battery_sub = orb_subscribe(ORB_ID(battery_status));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
airspeed_sub = orb_subscribe(ORB_ID(airspeed));
}
void build_eam_response(uint8_t *buffer, int *size)
@@ -81,6 +84,15 @@ void build_eam_response(uint8_t *buffer, int *size)
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* get a local copy of the current sensor values */
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
msg.speed_L = (uint8_t)speed & 0xff;
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
apps/sensors/sensor_params.c
+1 -1
View File
@@ -64,7 +64,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 2.5f);
PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
apps/sensors/sensors.cpp
+22 -8
View File
@@ -192,7 +192,7 @@ private:
float mag_scale[3];
float accel_offset[3];
float accel_scale[3];
float airspeed_offset;
int diff_pres_offset_pa;
int rc_type;
@@ -241,7 +241,7 @@ private:
param_t accel_scale[3];
param_t mag_offset[3];
param_t mag_scale[3];
param_t airspeed_offset;
param_t diff_pres_offset_pa;
param_t rc_map_roll;
param_t rc_map_pitch;
@@ -497,7 +497,7 @@ Sensors::Sensors() :
_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
/* Differential pressure offset */
_parameter_handles.airspeed_offset = param_find("SENS_DPRES_OFF");
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
@@ -707,7 +707,7 @@ Sensors::parameters_update()
param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2]));
/* Airspeed offset */
param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset));
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
@@ -910,15 +910,26 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
if (updated) {
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
raw.differential_pressure_counter++;
float airspeed_true = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f,
raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - 5.0f); //factor 1e2 for conversion from mBar to Pa
// XXX HACK - true temperature is much less than indicated temperature in baro,
// subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
float airspeed_indicated = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
raw.differential_pressure_counter++;
_airspeed.indicated_airspeed_m_s = airspeed_indicated;
_airspeed.true_airspeed_m_s = airspeed_true;
/* announce the airspeed if needed, just publish else */
if (_airspeed_pub > 0) {
orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
} else {
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed);
}
}
}
@@ -1080,7 +1091,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
*/
if (voltage > 0.4f) {
float diff_pres_pa = voltage * 1000.0f - _parameters.airspeed_offset; //for MPXV7002DP sensor
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
_diff_pres.timestamp = hrt_absolute_time();
_diff_pres.differential_pressure_pa = diff_pres_pa;
@@ -1330,6 +1341,7 @@ Sensors::task_main()
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -1405,6 +1417,8 @@ Sensors::task_main()
/* check battery voltage */
adc_poll(raw);
diff_pres_poll(raw);
/* Inform other processes that new data is available to copy */
if (_publishing)
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
apps/uORB/topics/differential_pressure.h
+4 -3
View File
@@ -52,9 +52,10 @@
* Differential pressure.
*/
struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
float differential_pressure_pa; /**< Differential pressure reading */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
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) */
};