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

Merge branch 'master' into navigator_new_vector

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-14 15:45:49 +01:00
parent 7bcddd192f b529e112b8
commit 97e4522c76
28 changed files with 984 additions and 273 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/module.mk
+1 -1
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2012-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
src/modules/sensors/sensor_params.c
+172 -15
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2012-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
@@ -38,6 +35,10 @@
* @file sensor_params.c
*
* Parameters defined by the sensors task.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/
#include <nuttx/config.h>
@@ -45,41 +46,98 @@
#include <systemlib/param/param.h>
/**
* Gyro X offset FIXME
* Gyro X offset
*
* This is an X-axis offset for the gyro.
* Adjust it according to the calibration data.
* This is an X-axis offset for the gyro. Adjust it according to the calibration data.
*
* @min -10.0
* @max 10.0
* @group Gyro Config
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
/**
* Gyro Y offset FIXME with dot.
* Gyro Y offset
*
* @min -10.0
* @max 10.0
* @group Gyro Config
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
/**
* Gyro Z offset FIXME
* Gyro Z offset
*
* @min -5.0
* @max 5.0
* @group Gyro Config
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
/**
* Gyro X scaling
*
* X-axis scaling.
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
/**
* Gyro Y scaling
*
* Y-axis scaling.
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
/**
* Gyro Z scaling
*
* Z-axis scaling.
*
* @min -1.5
* @max 1.5
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
/**
* Magnetometer X offset
*
* This is an X-axis offset for the magnetometer.
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
/**
* Magnetometer Y offset
*
* This is an Y-axis offset for the magnetometer.
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
/**
* Magnetometer Z offset
*
* This is an Z-axis offset for the magnetometer.
*
* @min -500.0
* @max 500.0
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f);
@@ -100,16 +158,114 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
/**
* RC Channel 1 Minimum
*
* Minimum value for RC channel 1
*
* @min 800.0
* @max 1500.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
/**
* RC Channel 1 Trim
*
* Mid point value (same as min for throttle)
*
* @min 800.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
/**
* RC Channel 1 Maximum
*
* Maximum value for RC channel 1
*
* @min 1500.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
/**
* RC Channel 1 Reverse
*
* Set to -1 to reverse channel.
*
* @min -1.0
* @max 1.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
/**
* RC Channel 1 dead zone
*
* The +- range of this value around the trim value will be considered as zero.
*
* @min 0.0
* @max 100.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
/**
* RC Channel 2 Minimum
*
* Minimum value for RC channel 2
*
* @min 800.0
* @max 1500.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f);
/**
* RC Channel 2 Trim
*
* Mid point value (same as min for throttle)
*
* @min 800.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f);
/**
* RC Channel 2 Maximum
*
* Maximum value for RC channel 2
*
* @min 1500.0
* @max 2200.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f);
/**
* RC Channel 2 Reverse
*
* Set to -1 to reverse channel.
*
* @min -1.0
* @max 1.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
/**
* RC Channel 2 dead zone
*
* The +- range of this value around the trim value will be considered as zero.
*
* @min 0.0
* @max 100.0
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f);
PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
@@ -223,6 +379,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
#endif
PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
src/modules/sensors/sensors.cpp
+62 -27
View File
@@ -114,6 +114,7 @@
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define ADC_BATTERY_CURRENT_CHANNEL -1
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
#endif
@@ -124,10 +125,8 @@
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
#endif
#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
#define BATT_V_LOWPASS 0.001f
#define BATT_V_IGNORE_THRESHOLD 3.5f
/**
* HACK - true temperature is much less than indicated temperature in baro,
@@ -215,6 +214,9 @@ private:
math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
struct {
float min[_rc_max_chan_count];
float trim[_rc_max_chan_count];
@@ -265,6 +267,7 @@ private:
float rc_fs_thr;
float battery_voltage_scaling;
float battery_current_scaling;
} _parameters; /**< local copies of interesting parameters */
@@ -314,6 +317,7 @@ private:
param_t rc_fs_thr;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
param_t board_rotation;
param_t external_mag_rotation;
@@ -465,7 +469,9 @@ Sensors::Sensors() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_mag_is_external(false)
_mag_is_external(false),
_battery_discharged(0),
_battery_current_timestamp(0)
{
/* basic r/c parameters */
@@ -558,6 +564,7 @@ Sensors::Sensors() :
_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
@@ -738,6 +745,11 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
/* scaling of ADC ticks to battery current */
if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
warnx("Failed updating current scaling param");
}
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
@@ -1155,17 +1167,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (!_publishing)
return;
hrt_abstime t = hrt_absolute_time();
/* rate limit to 100 Hz */
if (hrt_absolute_time() - _last_adc >= 10000) {
if (t - _last_adc >= 10000) {
/* make space for a maximum of eight channels */
struct adc_msg_s buf_adc[8];
/* read all channels available */
int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
if (ret >= (int)sizeof(buf_adc[0])) {
if (ret >= (int)sizeof(buf_adc[0])) {
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
/* Save raw voltage values */
if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
@@ -1176,27 +1187,40 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* Voltage in volts */
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
if (voltage > BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_v = voltage;
/* 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 (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_filtered_v = voltage;
}
_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;
_battery_status.timestamp = t;
_battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;
/* announce the battery voltage if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
} else {
/* mark status as invalid */
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
}
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
/* handle current only if voltage is valid */
if (_battery_status.voltage_v > 0.0f) {
float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
/* check measured current value */
if (current >= 0.0f) {
_battery_status.timestamp = t;
_battery_status.current_a = current;
if (_battery_current_timestamp != 0) {
/* initialize discharged value */
if (_battery_status.discharged_mah < 0.0f)
_battery_status.discharged_mah = 0.0f;
_battery_discharged += current * (t - _battery_current_timestamp);
_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
}
}
}
_battery_current_timestamp = t;
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1212,7 +1236,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
_diff_pres.timestamp = hrt_absolute_time();
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_pa = diff_pres_pa;
_diff_pres.voltage = voltage;
@@ -1225,8 +1249,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
}
}
_last_adc = t;
if (_battery_status.voltage_v > 0.0f) {
/* announce the battery status if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
_last_adc = hrt_absolute_time();
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
}
}
}
@@ -1514,7 +1546,10 @@ Sensors::task_main()
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = BAT_VOL_INITIAL;
_battery_status.voltage_v = 0.0f;
_battery_status.voltage_filtered_v = 0.0f;
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* get a set of initial values */
accel_poll(raw);