mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 15:28:53 +08:00
Merge branch 'master' into px4io-i2c
This commit is contained in:
px4dev
@@ -714,14 +714,17 @@ BMA180::measure()
|
||||
* perform only the axis assignment here.
|
||||
* Two non-value bits are discarded directly
|
||||
*/
|
||||
report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 1)) << 8) | (read_reg(ADDR_ACC_X_LSB)); // XXX PX4DEV raw_report.x;
|
||||
report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 3)) << 8) | (read_reg(ADDR_ACC_X_LSB + 2)); // XXX PX4DEV raw_report.y;
|
||||
report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB + 5)) << 8) | (read_reg(ADDR_ACC_X_LSB + 4)); // XXX PX4DEV raw_report.z;
|
||||
report->y_raw = read_reg(ADDR_ACC_X_LSB + 0);
|
||||
report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
|
||||
report->x_raw = read_reg(ADDR_ACC_X_LSB + 2);
|
||||
report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
|
||||
report->z_raw = read_reg(ADDR_ACC_X_LSB + 4);
|
||||
report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
|
||||
|
||||
/* discard two non-value bits in the 16 bit measurement */
|
||||
report->x_raw = (report->x_raw >> 2);
|
||||
report->y_raw = (report->y_raw >> 2);
|
||||
report->z_raw = (report->z_raw >> 2);
|
||||
report->x_raw = (report->x_raw / 4);
|
||||
report->y_raw = (report->y_raw / 4);
|
||||
report->z_raw = (report->z_raw / 4);
|
||||
|
||||
/* invert y axis, due to 14 bit data no overflow can occur in the negation */
|
||||
report->y_raw = -report->y_raw;
|
||||
|
||||
+75
-26
@@ -67,6 +67,7 @@
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
#include <systemlib/ppm_decode.h>
|
||||
#include <systemlib/airspeed.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
@@ -75,6 +76,7 @@
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
|
||||
#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
|
||||
#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
|
||||
@@ -88,9 +90,10 @@
|
||||
#define BARO_HEALTH_COUNTER_LIMIT_OK 5
|
||||
#define ADC_HEALTH_COUNTER_LIMIT_OK 5
|
||||
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
|
||||
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
|
||||
|
||||
#define BAT_VOL_INITIAL 12.f
|
||||
#define BAT_VOL_INITIAL 0.f
|
||||
#define BAT_VOL_LOWPASS_1 0.99f
|
||||
#define BAT_VOL_LOWPASS_2 0.01f
|
||||
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
|
||||
@@ -161,11 +164,14 @@ private:
|
||||
orb_advert_t _manual_control_pub; /**< manual control signal topic */
|
||||
orb_advert_t _rc_pub; /**< raw r/c control topic */
|
||||
orb_advert_t _battery_pub; /**< battery status */
|
||||
orb_advert_t _airspeed_pub; /**< airspeed */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
struct rc_channels_s _rc; /**< r/c channel data */
|
||||
struct battery_status_s _battery_status; /**< battery status */
|
||||
struct baro_report _barometer; /**< barometer data */
|
||||
struct differential_pressure_s _differential_pressure;
|
||||
|
||||
struct {
|
||||
float min[_rc_max_chan_count];
|
||||
@@ -389,6 +395,7 @@ Sensors::Sensors() :
|
||||
_manual_control_pub(-1),
|
||||
_rc_pub(-1),
|
||||
_battery_pub(-1),
|
||||
_airspeed_pub(-1),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
|
||||
@@ -861,13 +868,12 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
|
||||
orb_check(_baro_sub, &baro_updated);
|
||||
|
||||
if (baro_updated) {
|
||||
struct baro_report baro_report;
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report);
|
||||
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
|
||||
|
||||
raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar
|
||||
raw.baro_alt_meter = baro_report.altitude; // Altitude in meters
|
||||
raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
|
||||
raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar
|
||||
raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
|
||||
raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius
|
||||
|
||||
raw.baro_counter++;
|
||||
}
|
||||
@@ -988,29 +994,72 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
||||
|
||||
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
|
||||
|
||||
if (ret >= sizeof(buf_adc[0]) && ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
|
||||
/* Voltage in volts */
|
||||
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
|
||||
if (ret >= (int)sizeof(buf_adc[0])) {
|
||||
|
||||
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
|
||||
if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
|
||||
/* Voltage in volts */
|
||||
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
|
||||
|
||||
/* one-time initialization of low-pass value to avoid long init delays */
|
||||
if (_battery_status.voltage_v < 3.0f) {
|
||||
_battery_status.voltage_v = voltage;
|
||||
}
|
||||
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
|
||||
|
||||
_battery_status.timestamp = hrt_absolute_time();
|
||||
_battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
|
||||
/* current and discharge are unknown */
|
||||
_battery_status.current_a = -1.0f;
|
||||
_battery_status.discharged_mah = -1.0f;
|
||||
/* one-time initialization of low-pass value to avoid long init delays */
|
||||
if (_battery_status.voltage_v < 3.0f) {
|
||||
_battery_status.voltage_v = voltage;
|
||||
}
|
||||
|
||||
/* announce the battery voltage if needed, just publish else */
|
||||
if (_battery_pub > 0) {
|
||||
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
|
||||
_battery_status.timestamp = hrt_absolute_time();
|
||||
_battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
|
||||
/* current and discharge are unknown */
|
||||
_battery_status.current_a = -1.0f;
|
||||
_battery_status.discharged_mah = -1.0f;
|
||||
|
||||
} else {
|
||||
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
|
||||
/* announce the battery voltage if needed, just publish else */
|
||||
if (_battery_pub > 0) {
|
||||
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
|
||||
|
||||
} else {
|
||||
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
|
||||
}
|
||||
}
|
||||
|
||||
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
|
||||
|
||||
/* calculate airspeed, raw is the difference from */
|
||||
|
||||
float voltage = buf_adc[i].am_data / 4096.0f;
|
||||
|
||||
/**
|
||||
* The voltage divider pulls the signal down, only act on
|
||||
* a valid voltage from a connected sensor
|
||||
*/
|
||||
if (voltage > 0.4f) {
|
||||
|
||||
float pres_raw = fabsf(voltage - (3.3f / 2.0f));
|
||||
float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f;
|
||||
|
||||
float airspeed_true = calc_true_airspeed(pres_mbar + _barometer.pressure,
|
||||
_barometer.pressure, _barometer.temperature - 5.0f);
|
||||
// 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(pres_mbar + _barometer.pressure,
|
||||
_barometer.pressure, _barometer.temperature - 5.0f);
|
||||
// XXX HACK
|
||||
|
||||
_differential_pressure.timestamp = hrt_absolute_time();
|
||||
_differential_pressure.static_pressure_mbar = _barometer.pressure;
|
||||
_differential_pressure.differential_pressure_mbar = pres_mbar;
|
||||
_differential_pressure.temperature_celcius = _barometer.temperature;
|
||||
_differential_pressure.indicated_airspeed_m_s = airspeed_indicated;
|
||||
_differential_pressure.true_airspeed_m_s = airspeed_true;
|
||||
|
||||
/* announce the airspeed if needed, just publish else */
|
||||
if (_airspeed_pub > 0) {
|
||||
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_differential_pressure);
|
||||
|
||||
} else {
|
||||
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_differential_pressure);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1114,7 +1163,7 @@ Sensors::ppm_poll()
|
||||
}
|
||||
|
||||
/* reverse channel if required */
|
||||
if (i == _rc.function[THROTTLE]) {
|
||||
if (i == (int)_rc.function[THROTTLE]) {
|
||||
if ((int)_parameters.rev[i] == -1) {
|
||||
_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
|
||||
}
|
||||
|
||||
@@ -61,6 +61,7 @@ static void do_load(const char* param_file_name);
|
||||
static void do_import(const char* param_file_name);
|
||||
static void do_show(const char* search_string);
|
||||
static void do_show_print(void *arg, param_t param);
|
||||
static void do_set(const char* name, const char* val);
|
||||
|
||||
int
|
||||
param_main(int argc, char *argv[])
|
||||
@@ -100,15 +101,24 @@ param_main(int argc, char *argv[])
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "show"))
|
||||
if (!strcmp(argv[1], "show")) {
|
||||
if (argc >= 3) {
|
||||
do_show(argv[2]);
|
||||
} else {
|
||||
do_show(NULL);
|
||||
}
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "set")) {
|
||||
if (argc >= 4) {
|
||||
do_set(argv[2], argv[3]);
|
||||
} else {
|
||||
errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
errx(1, "expected a command, try 'load', 'import', 'show', 'select' or 'save'");
|
||||
errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'");
|
||||
}
|
||||
|
||||
static void
|
||||
@@ -185,9 +195,11 @@ do_show_print(void *arg, param_t param)
|
||||
float f;
|
||||
const char *search_string = (const char*)arg;
|
||||
|
||||
/* print nothing if search string valid and not matching */
|
||||
if (arg != NULL && (strcmp(search_string, param_name(param) != 0)))
|
||||
/* print nothing if search string is invalid and not matching */
|
||||
if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) {
|
||||
/* param not found */
|
||||
return;
|
||||
}
|
||||
|
||||
printf("%c %s: ",
|
||||
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
|
||||
@@ -225,3 +237,61 @@ do_show_print(void *arg, param_t param)
|
||||
|
||||
printf("<error fetching parameter %d>\n", param);
|
||||
}
|
||||
|
||||
static void
|
||||
do_set(const char* name, const char* val)
|
||||
{
|
||||
int32_t i;
|
||||
float f;
|
||||
param_t param = param_find(name);
|
||||
|
||||
/* set nothing if parameter cannot be found */
|
||||
if (param == PARAM_INVALID) {
|
||||
/* param not found */
|
||||
errx(1, "Error: Parameter %s not found.", name);
|
||||
}
|
||||
|
||||
printf("%c %s: ",
|
||||
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
|
||||
param_name(param));
|
||||
|
||||
/*
|
||||
* Set parameter if type is known and conversion from string to value turns out fine
|
||||
*/
|
||||
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_INT32:
|
||||
if (!param_get(param, &i)) {
|
||||
printf("old: %d", i);
|
||||
|
||||
/* convert string */
|
||||
char* end;
|
||||
i = strtol(val,&end,10);
|
||||
param_set(param, &i);
|
||||
printf(" -> new: %d\n", i);
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_FLOAT:
|
||||
if (!param_get(param, &f)) {
|
||||
printf("float values are not yet supported.");
|
||||
// printf("old: %4.4f", (double)f);
|
||||
|
||||
// /* convert string */
|
||||
// char* end;
|
||||
// f = strtof(val,&end);
|
||||
// param_set(param, &f);
|
||||
// printf(" -> new: %4.4f\n", f);
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@@ -43,7 +43,8 @@ CSRCS = err.c \
|
||||
conversions.c \
|
||||
cpuload.c \
|
||||
getopt_long.c \
|
||||
up_cxxinitialize.c
|
||||
up_cxxinitialize.c \
|
||||
airspeed.c
|
||||
|
||||
# ppm_decode.c \
|
||||
|
||||
|
||||
@@ -41,11 +41,13 @@
|
||||
*/
|
||||
|
||||
#include "math.h"
|
||||
#include "conversions.h"
|
||||
#include "airspeed.h"
|
||||
|
||||
|
||||
float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature)
|
||||
{
|
||||
return sqrtf((2.0f*(pressure_front - pressure_ambient)) / air_density_sea_level);
|
||||
return sqrtf((2.0f*(pressure_front - pressure_ambient)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -60,7 +62,7 @@ float calc_indicated_airspeed(float pressure_front, float pressure_ambient, floa
|
||||
*/
|
||||
float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature)
|
||||
{
|
||||
return speed * sqrtf(air_density_sea_level / get_air_density(pressure_ambient, temperature));
|
||||
return speed * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -76,4 +78,4 @@ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, flo
|
||||
float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature)
|
||||
{
|
||||
return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -33,13 +33,16 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file airspeed.c
|
||||
* Airspeed estimation
|
||||
* @file airspeed.h
|
||||
* Airspeed estimation declarations
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef AIRSPEED_H_
|
||||
#define AIRSPEED_H_
|
||||
|
||||
#include "math.h"
|
||||
#include "conversions.h"
|
||||
|
||||
@@ -83,4 +86,6 @@ __EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_amb
|
||||
*/
|
||||
__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature);
|
||||
|
||||
__END_DECLS
|
||||
__END_DECLS
|
||||
|
||||
#endif
|
||||
|
||||
@@ -42,10 +42,6 @@
|
||||
|
||||
#include "conversions.h"
|
||||
|
||||
#define air_gas_constant 8.31432f
|
||||
#define air_density_sea_level 1.225f
|
||||
#define absolute_null_kelvin 273.15f
|
||||
|
||||
int16_t
|
||||
int16_t_from_bytes(uint8_t bytes[])
|
||||
{
|
||||
@@ -154,5 +150,5 @@ void quat2rot(const float Q[4], float R[9])
|
||||
|
||||
float get_air_density(float static_pressure, float temperature_celsius)
|
||||
{
|
||||
return static_pressure / (air_gas_constant * (temperature_celsius + absolute_null_kelvin));
|
||||
return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius + CONSTANTS_ABSOLUTE_NULL_KELVIN));
|
||||
}
|
||||
|
||||
@@ -44,7 +44,10 @@
|
||||
#include <float.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define CONSTANTS_ONE_G 9.80665f
|
||||
#define CONSTANTS_ONE_G 9.80665f
|
||||
#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f
|
||||
#define CONSTANTS_AIR_GAS_CONST 8.31432f
|
||||
#define CONSTANTS_ABSOLUTE_NULL_KELVIN 273.15f
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
|
||||
Reference in New Issue
Block a user