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

merge master

Browse Source
This commit is contained in:
Lorenz Meier
2014-01-31 09:51:59 +01:00
parent fb446c01b8 83df116c7a
commit d2b183c05b
121 changed files with 4043 additions and 3593 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensor_params.c
+62 -2
View File
@@ -379,15 +379,75 @@ 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 */
/**
* Roll control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading roll inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
/**
* Pitch control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading pitch inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
/**
* Throttle control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading throttle inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
/**
* Yaw control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading yaw inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
/**
* Mode switch channel mapping.
*
* This is the main flight mode selector.
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for deciding about the main mode.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
src/modules/sensors/sensors.cpp
+68 -35
View File
@@ -115,6 +115,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
@@ -125,10 +126,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,
@@ -216,6 +215,9 @@ private:
math::Matrix _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];
@@ -266,6 +268,7 @@ private:
float rc_fs_thr;
float battery_voltage_scaling;
float battery_current_scaling;
} _parameters; /**< local copies of interesting parameters */
@@ -315,6 +318,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;
@@ -468,7 +472,9 @@ Sensors::Sensors() :
_board_rotation(3, 3),
_external_mag_rotation(3, 3),
_mag_is_external(false)
_mag_is_external(false),
_battery_discharged(0),
_battery_current_timestamp(0)
{
/* basic r/c parameters */
@@ -561,6 +567,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");
@@ -741,6 +748,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));
@@ -786,7 +798,6 @@ Sensors::accel_init()
#endif
warnx("using system accel");
close(fd);
}
}
@@ -826,7 +837,6 @@ Sensors::gyro_init()
#endif
warnx("using system gyro");
close(fd);
}
}
@@ -1158,17 +1168,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);
@@ -1179,27 +1188,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) {
@@ -1215,7 +1237,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;
@@ -1228,8 +1250,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);
}
}
}
}
@@ -1247,6 +1277,9 @@ Sensors::rc_poll()
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
if (rc_input.rc_lost)
return;
struct manual_control_setpoint_s manual_control;
struct actuator_controls_s actuator_group_3;
@@ -1291,7 +1324,7 @@ Sensors::rc_poll()
channel_limit = _rc_max_chan_count;
/* we are accepting this message */
_rc_last_valid = rc_input.timestamp;
_rc_last_valid = rc_input.timestamp_last_signal;
/* Read out values from raw message */
for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1340,9 +1373,9 @@ Sensors::rc_poll()
}
_rc.chan_count = rc_input.channel_count;
_rc.timestamp = rc_input.timestamp;
_rc.timestamp = rc_input.timestamp_last_signal;
manual_control.timestamp = rc_input.timestamp;
manual_control.timestamp = rc_input.timestamp_last_signal;
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
@@ -1476,9 +1509,6 @@ void
Sensors::task_main()
{
/* inform about start */
warnx("Initializing..");
/* start individual sensors */
accel_init();
gyro_init();
@@ -1517,7 +1547,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 = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* get a set of initial values */
accel_poll(raw);