zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

sensors: break off parameters initialization into separate source file

sbg
Beat Küng 9 years ago committed by Lorenz Meier
parent
c5e485bdd7
commit
f9b3b5a799
5 changed files with 487 additions and 406 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      src/modules/sensors/CMakeLists.txt
  2. 465
      src/modules/sensors/parameters.cpp
  3. 12
      src/modules/sensors/parameters.h
  4. 410
      src/modules/sensors/sensors.cpp
  5. 5
      src/platforms/px4_defines.h

1
src/modules/sensors/CMakeLists.txt
Unescape View File

@ -41,6 +41,7 @@ px4_add_module( @@ -41,6 +41,7 @@ px4_add_module(
rc_update.cpp
sensors.cpp
sensors_init.cpp
parameters.cpp
DEPENDS
platforms__common

465
src/modules/sensors/parameters.cpp
Unescape View File

@ -0,0 +1,465 @@ @@ -0,0 +1,465 @@
/****************************************************************************
*
* Copyright (c) 2016 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 parameters.cpp
*
* @author Beat Kueng <beat-kueng@gmx.net>
*/
#include "parameters.h"
namespace sensors
{
int initialize_parameter_handles(ParameterHandles &parameter_handles)
{
/* basic r/c parameters */
for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) {
char nbuf[16];
/* min values */
sprintf(nbuf, "RC%d_MIN", i + 1);
parameter_handles.min[i] = param_find(nbuf);
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
parameter_handles.trim[i] = param_find(nbuf);
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
parameter_handles.max[i] = param_find(nbuf);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
parameter_handles.rev[i] = param_find(nbuf);
/* channel deadzone */
sprintf(nbuf, "RC%d_DZ", i + 1);
parameter_handles.dz[i] = param_find(nbuf);
}
/* mandatory input switched, mapped to channels 1-4 per default */
parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL");
parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW");
parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE");
/* mandatory mode switches, mapped to channel 5 and 6 per default */
parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");
parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
/* optional mode switches, not mapped per default */
parameter_handles.rc_map_rattitude_sw = param_find("RC_MAP_RATT_SW");
parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW");
parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW");
parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_SW");
parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3");
parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
/* RC to parameter mapping for changing parameters with RC */
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
char name[rc_parameter_map_s::PARAM_ID_LEN];
snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d",
i + 1); // shifted by 1 because param name starts at 1
parameter_handles.rc_map_param[i] = param_find(name);
}
parameter_handles.rc_map_flightmode = param_find("RC_MAP_FLTMODE");
/* RC thresholds */
parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
parameter_handles.rc_auto_th = param_find("RC_AUTO_TH");
parameter_handles.rc_rattitude_th = param_find("RC_RATT_TH");
parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH");
parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH");
parameter_handles.rc_trans_th = param_find("RC_TRANS_TH");
parameter_handles.rc_gear_th = param_find("RC_GEAR_TH");
/* Differential pressure offset */
parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
parameter_handles.battery_voltage_scaling = param_find("BAT_CNT_V_VOLT");
parameter_handles.battery_current_scaling = param_find("BAT_CNT_V_CURR");
parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR");
parameter_handles.battery_v_div = param_find("BAT_V_DIV");
parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V");
parameter_handles.battery_source = param_find("BAT_SOURCE");
/* rotations */
parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
/* rotation offsets */
parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
/* Barometer QNH */
parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
parameter_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
// These are parameters for which QGroundControl always expects to be returned in a list request.
// We do a param_find here to force them into the list.
(void)param_find("RC_CHAN_CNT");
(void)param_find("RC_TH_USER");
(void)param_find("CAL_MAG0_ID");
(void)param_find("CAL_MAG1_ID");
(void)param_find("CAL_MAG2_ID");
(void)param_find("CAL_MAG0_ROT");
(void)param_find("CAL_MAG1_ROT");
(void)param_find("CAL_MAG2_ROT");
(void)param_find("CAL_MAG_SIDES");
(void)param_find("CAL_MAG1_XOFF");
(void)param_find("CAL_MAG1_XSCALE");
(void)param_find("CAL_MAG1_YOFF");
(void)param_find("CAL_MAG1_YSCALE");
(void)param_find("CAL_MAG1_ZOFF");
(void)param_find("CAL_MAG1_ZSCALE");
(void)param_find("CAL_MAG2_XOFF");
(void)param_find("CAL_MAG2_XSCALE");
(void)param_find("CAL_MAG2_YOFF");
(void)param_find("CAL_MAG2_YSCALE");
(void)param_find("CAL_MAG2_ZOFF");
(void)param_find("CAL_MAG2_ZSCALE");
(void)param_find("CAL_GYRO1_XOFF");
(void)param_find("CAL_GYRO1_XSCALE");
(void)param_find("CAL_GYRO1_YOFF");
(void)param_find("CAL_GYRO1_YSCALE");
(void)param_find("CAL_GYRO1_ZOFF");
(void)param_find("CAL_GYRO1_ZSCALE");
(void)param_find("CAL_GYRO2_XOFF");
(void)param_find("CAL_GYRO2_XSCALE");
(void)param_find("CAL_GYRO2_YOFF");
(void)param_find("CAL_GYRO2_YSCALE");
(void)param_find("CAL_GYRO2_ZOFF");
(void)param_find("CAL_GYRO2_ZSCALE");
(void)param_find("CAL_ACC1_XOFF");
(void)param_find("CAL_ACC1_XSCALE");
(void)param_find("CAL_ACC1_YOFF");
(void)param_find("CAL_ACC1_YSCALE");
(void)param_find("CAL_ACC1_ZOFF");
(void)param_find("CAL_ACC1_ZSCALE");
(void)param_find("CAL_ACC2_XOFF");
(void)param_find("CAL_ACC2_XSCALE");
(void)param_find("CAL_ACC2_YOFF");
(void)param_find("CAL_ACC2_YSCALE");
(void)param_find("CAL_ACC2_ZOFF");
(void)param_find("CAL_ACC2_ZSCALE");
(void)param_find("SYS_PARAM_VER");
(void)param_find("SYS_AUTOSTART");
(void)param_find("SYS_AUTOCONFIG");
(void)param_find("PWM_RATE");
(void)param_find("PWM_MIN");
(void)param_find("PWM_MAX");
(void)param_find("PWM_DISARMED");
(void)param_find("PWM_AUX_MIN");
(void)param_find("PWM_AUX_MAX");
(void)param_find("PWM_AUX_DISARMED");
(void)param_find("TRIG_MODE");
(void)param_find("UAVCAN_ENABLE");
(void)param_find("SYS_MC_EST_GROUP");
return 0;
}
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters)
{
bool rc_valid = true;
float tmpScaleFactor = 0.0f;
float tmpRevFactor = 0.0f;
int ret = PX4_OK;
/* rc values */
for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) {
param_get(parameter_handles.min[i], &(parameters.min[i]));
param_get(parameter_handles.trim[i], &(parameters.trim[i]));
param_get(parameter_handles.max[i], &(parameters.max[i]));
param_get(parameter_handles.rev[i], &(parameters.rev[i]));
param_get(parameter_handles.dz[i], &(parameters.dz[i]));
tmpScaleFactor = (1.0f / ((parameters.max[i] - parameters.min[i]) / 2.0f) * parameters.rev[i]);
tmpRevFactor = tmpScaleFactor * parameters.rev[i];
/* handle blowup in the scaling factor calculation */
if (!PX4_ISFINITE(tmpScaleFactor) ||
(tmpRevFactor < 0.000001f) ||
(tmpRevFactor > 0.2f)) {
PX4_WARN("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(parameters.rev[i]));
/* scaling factors do not make sense, lock them down */
parameters.scaling_factor[i] = 0.0f;
rc_valid = false;
} else {
parameters.scaling_factor[i] = tmpScaleFactor;
}
}
/* handle wrong values */
if (!rc_valid) {
PX4_ERR("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
}
const char *paramerr = "FAIL PARM LOAD";
/* channel mapping */
if (param_get(parameter_handles.rc_map_roll, &(parameters.rc_map_roll)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_pitch, &(parameters.rc_map_pitch)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_yaw, &(parameters.rc_map_yaw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_throttle, &(parameters.rc_map_throttle)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_failsafe, &(parameters.rc_map_failsafe)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_mode_sw, &(parameters.rc_map_mode_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_return_sw, &(parameters.rc_map_return_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_rattitude_sw, &(parameters.rc_map_rattitude_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_posctl_sw, &(parameters.rc_map_posctl_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_loiter_sw, &(parameters.rc_map_loiter_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_acro_sw, &(parameters.rc_map_acro_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_offboard_sw, &(parameters.rc_map_offboard_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_kill_sw, &(parameters.rc_map_kill_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_trans_sw, &(parameters.rc_map_trans_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_flaps, &(parameters.rc_map_flaps)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.rc_map_gear_sw, &(parameters.rc_map_gear_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
param_get(parameter_handles.rc_map_aux1, &(parameters.rc_map_aux1));
param_get(parameter_handles.rc_map_aux2, &(parameters.rc_map_aux2));
param_get(parameter_handles.rc_map_aux3, &(parameters.rc_map_aux3));
param_get(parameter_handles.rc_map_aux4, &(parameters.rc_map_aux4));
param_get(parameter_handles.rc_map_aux5, &(parameters.rc_map_aux5));
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
param_get(parameter_handles.rc_map_param[i], &(parameters.rc_map_param[i]));
}
param_get(parameter_handles.rc_map_flightmode, &(parameters.rc_map_flightmode));
param_get(parameter_handles.rc_fails_thr, &(parameters.rc_fails_thr));
param_get(parameter_handles.rc_assist_th, &(parameters.rc_assist_th));
parameters.rc_assist_inv = (parameters.rc_assist_th < 0);
parameters.rc_assist_th = fabs(parameters.rc_assist_th);
param_get(parameter_handles.rc_auto_th, &(parameters.rc_auto_th));
parameters.rc_auto_inv = (parameters.rc_auto_th < 0);
parameters.rc_auto_th = fabs(parameters.rc_auto_th);
param_get(parameter_handles.rc_rattitude_th, &(parameters.rc_rattitude_th));
parameters.rc_rattitude_inv = (parameters.rc_rattitude_th < 0);
parameters.rc_rattitude_th = fabs(parameters.rc_rattitude_th);
param_get(parameter_handles.rc_posctl_th, &(parameters.rc_posctl_th));
parameters.rc_posctl_inv = (parameters.rc_posctl_th < 0);
parameters.rc_posctl_th = fabs(parameters.rc_posctl_th);
param_get(parameter_handles.rc_return_th, &(parameters.rc_return_th));
parameters.rc_return_inv = (parameters.rc_return_th < 0);
parameters.rc_return_th = fabs(parameters.rc_return_th);
param_get(parameter_handles.rc_loiter_th, &(parameters.rc_loiter_th));
parameters.rc_loiter_inv = (parameters.rc_loiter_th < 0);
parameters.rc_loiter_th = fabs(parameters.rc_loiter_th);
param_get(parameter_handles.rc_acro_th, &(parameters.rc_acro_th));
parameters.rc_acro_inv = (parameters.rc_acro_th < 0);
parameters.rc_acro_th = fabs(parameters.rc_acro_th);
param_get(parameter_handles.rc_offboard_th, &(parameters.rc_offboard_th));
parameters.rc_offboard_inv = (parameters.rc_offboard_th < 0);
parameters.rc_offboard_th = fabs(parameters.rc_offboard_th);
param_get(parameter_handles.rc_killswitch_th, &(parameters.rc_killswitch_th));
parameters.rc_killswitch_inv = (parameters.rc_killswitch_th < 0);
parameters.rc_killswitch_th = fabs(parameters.rc_killswitch_th);
param_get(parameter_handles.rc_trans_th, &(parameters.rc_trans_th));
parameters.rc_trans_inv = (parameters.rc_trans_th < 0);
parameters.rc_trans_th = fabs(parameters.rc_trans_th);
param_get(parameter_handles.rc_gear_th, &(parameters.rc_gear_th));
parameters.rc_gear_inv = (parameters.rc_gear_th < 0);
parameters.rc_gear_th = fabs(parameters.rc_gear_th);
/* Airspeed offset */
param_get(parameter_handles.diff_pres_offset_pa, &(parameters.diff_pres_offset_pa));
param_get(parameter_handles.diff_pres_analog_scale, &(parameters.diff_pres_analog_scale));
/* scaling of ADC ticks to battery voltage */
if (param_get(parameter_handles.battery_voltage_scaling, &(parameters.battery_voltage_scaling)) != OK) {
PX4_WARN("%s", paramerr);
} else if (parameters.battery_voltage_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_voltage_scaling = (3.3f / 4096);
param_set(parameter_handles.battery_voltage_scaling, &parameters.battery_voltage_scaling);
}
/* scaling of ADC ticks to battery current */
if (param_get(parameter_handles.battery_current_scaling, &(parameters.battery_current_scaling)) != OK) {
PX4_WARN("%s", paramerr);
} else if (parameters.battery_current_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_current_scaling = (3.3f / 4096);
param_set(parameter_handles.battery_current_scaling, &parameters.battery_current_scaling);
}
if (param_get(parameter_handles.battery_current_offset, &(parameters.battery_current_offset)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.battery_v_div, &(parameters.battery_v_div)) != OK) {
PX4_WARN("%s", paramerr);
parameters.battery_v_div = 0.0f;
} else if (parameters.battery_v_div <= 0.0f) {
/* apply scaling according to defaults if set to default */
#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
parameters.battery_v_div = 13.653333333f;
#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 )
parameters.battery_v_div = 10.177939394f;
#elif defined (CONFIG_ARCH_BOARD_AEROCORE)
parameters.battery_v_div = 7.8196363636f;
#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
parameters.battery_v_div = 5.7013919372f;
#elif defined (CONFIG_ARCH_BOARD_SITL)
parameters.battery_v_div = 10.177939394f;
#elif defined (CONFIG_ARCH_BOARD_TAP_V1)
parameters.battery_v_div = 9.0f;
#else
/* ensure a missing default trips a low voltage lockdown */
parameters.battery_v_div = 0.0f;
#endif
param_set(parameter_handles.battery_v_div, &parameters.battery_v_div);
}
if (param_get(parameter_handles.battery_a_per_v, &(parameters.battery_a_per_v)) != OK) {
PX4_WARN("%s", paramerr);
parameters.battery_a_per_v = 0.0f;
} else if (parameters.battery_a_per_v <= 0.0f) {
/* apply scaling according to defaults if set to default */
#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
/* current scaling for ACSP4 */
parameters.battery_a_per_v = 36.367515152f;
#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_MINDPX_V2) || defined (CONFIG_ARCH_BOARD_AEROCORE) || defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
/* current scaling for 3DR power brick */
parameters.battery_a_per_v = 15.391030303f;
#elif defined (CONFIG_ARCH_BOARD_SITL)
parameters.battery_a_per_v = 15.391030303f;
#else
/* ensure a missing default leads to an unrealistic current value */
parameters.battery_a_per_v = 0.0f;
#endif
param_set(parameter_handles.battery_a_per_v, &parameters.battery_a_per_v);
}
param_get(parameter_handles.battery_source, &(parameters.battery_source));
param_get(parameter_handles.board_rotation, &(parameters.board_rotation));
param_get(parameter_handles.board_offset[0], &(parameters.board_offset[0]));
param_get(parameter_handles.board_offset[1], &(parameters.board_offset[1]));
param_get(parameter_handles.board_offset[2], &(parameters.board_offset[2]));
param_get(parameter_handles.baro_qnh, &(parameters.baro_qnh));
param_get(parameter_handles.vibe_thresh, &parameters.vibration_warning_threshold);
return ret;
}
} /* namespace sensors */

12
src/modules/sensors/parameters.h
Unescape View File

@ -208,5 +208,17 @@ struct ParameterHandles { @@ -208,5 +208,17 @@ struct ParameterHandles {
};
/**
* initialize ParameterHandles struct
* @return 0 on succes, <0 on error
*/
int initialize_parameter_handles(ParameterHandles &parameter_handles);
/**
* Read out the parameters using the handles into the parameters struct.
* @return 0 on succes, <0 on error
*/
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters);
} /* namespace sensors */

410
src/modules/sensors/sensors.cpp
Unescape View File

@ -437,177 +437,7 @@ Sensors::Sensors() : @@ -437,177 +437,7 @@ Sensors::Sensors() :
memset(&_accel_diff, 0, sizeof(_accel_diff));
memset(&_gyro_diff, 0, sizeof(_gyro_diff));
/* basic r/c parameters */
for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) {
char nbuf[16];
/* min values */
sprintf(nbuf, "RC%d_MIN", i + 1);
_parameter_handles.min[i] = param_find(nbuf);
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
_parameter_handles.trim[i] = param_find(nbuf);
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
_parameter_handles.max[i] = param_find(nbuf);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
_parameter_handles.rev[i] = param_find(nbuf);
/* channel deadzone */
sprintf(nbuf, "RC%d_DZ", i + 1);
_parameter_handles.dz[i] = param_find(nbuf);
}
/* mandatory input switched, mapped to channels 1-4 per default */
_parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL");
_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
_parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW");
_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
_parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE");
/* mandatory mode switches, mapped to channel 5 and 6 per default */
_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
_parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");
_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
/* optional mode switches, not mapped per default */
_parameter_handles.rc_map_rattitude_sw = param_find("RC_MAP_RATT_SW");
_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
_parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
_parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW");
_parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW");
_parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_SW");
_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
_parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3");
_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
/* RC to parameter mapping for changing parameters with RC */
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
char name[rc_parameter_map_s::PARAM_ID_LEN];
snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d",
i + 1); // shifted by 1 because param name starts at 1
_parameter_handles.rc_map_param[i] = param_find(name);
}
_parameter_handles.rc_map_flightmode = param_find("RC_MAP_FLTMODE");
/* RC thresholds */
_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
_parameter_handles.rc_auto_th = param_find("RC_AUTO_TH");
_parameter_handles.rc_rattitude_th = param_find("RC_RATT_TH");
_parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH");
_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
_parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
_parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH");
_parameter_handles.rc_trans_th = param_find("RC_TRANS_TH");
_parameter_handles.rc_gear_th = param_find("RC_GEAR_TH");
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
_parameter_handles.battery_voltage_scaling = param_find("BAT_CNT_V_VOLT");
_parameter_handles.battery_current_scaling = param_find("BAT_CNT_V_CURR");
_parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR");
_parameter_handles.battery_v_div = param_find("BAT_V_DIV");
_parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V");
_parameter_handles.battery_source = param_find("BAT_SOURCE");
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
/* rotation offsets */
_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
/* Barometer QNH */
_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
_parameter_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
// These are parameters for which QGroundControl always expects to be returned in a list request.
// We do a param_find here to force them into the list.
(void)param_find("RC_CHAN_CNT");
(void)param_find("RC_TH_USER");
(void)param_find("CAL_MAG0_ID");
(void)param_find("CAL_MAG1_ID");
(void)param_find("CAL_MAG2_ID");
(void)param_find("CAL_MAG0_ROT");
(void)param_find("CAL_MAG1_ROT");
(void)param_find("CAL_MAG2_ROT");
(void)param_find("CAL_MAG_SIDES");
(void)param_find("CAL_MAG1_XOFF");
(void)param_find("CAL_MAG1_XSCALE");
(void)param_find("CAL_MAG1_YOFF");
(void)param_find("CAL_MAG1_YSCALE");
(void)param_find("CAL_MAG1_ZOFF");
(void)param_find("CAL_MAG1_ZSCALE");
(void)param_find("CAL_MAG2_XOFF");
(void)param_find("CAL_MAG2_XSCALE");
(void)param_find("CAL_MAG2_YOFF");
(void)param_find("CAL_MAG2_YSCALE");
(void)param_find("CAL_MAG2_ZOFF");
(void)param_find("CAL_MAG2_ZSCALE");
(void)param_find("CAL_GYRO1_XOFF");
(void)param_find("CAL_GYRO1_XSCALE");
(void)param_find("CAL_GYRO1_YOFF");
(void)param_find("CAL_GYRO1_YSCALE");
(void)param_find("CAL_GYRO1_ZOFF");
(void)param_find("CAL_GYRO1_ZSCALE");
(void)param_find("CAL_GYRO2_XOFF");
(void)param_find("CAL_GYRO2_XSCALE");
(void)param_find("CAL_GYRO2_YOFF");
(void)param_find("CAL_GYRO2_YSCALE");
(void)param_find("CAL_GYRO2_ZOFF");
(void)param_find("CAL_GYRO2_ZSCALE");
(void)param_find("CAL_ACC1_XOFF");
(void)param_find("CAL_ACC1_XSCALE");
(void)param_find("CAL_ACC1_YOFF");
(void)param_find("CAL_ACC1_YSCALE");
(void)param_find("CAL_ACC1_ZOFF");
(void)param_find("CAL_ACC1_ZSCALE");
(void)param_find("CAL_ACC2_XOFF");
(void)param_find("CAL_ACC2_XSCALE");
(void)param_find("CAL_ACC2_YOFF");
(void)param_find("CAL_ACC2_YSCALE");
(void)param_find("CAL_ACC2_ZOFF");
(void)param_find("CAL_ACC2_ZSCALE");
(void)param_find("SYS_PARAM_VER");
(void)param_find("SYS_AUTOSTART");
(void)param_find("SYS_AUTOCONFIG");
(void)param_find("PWM_RATE");
(void)param_find("PWM_MIN");
(void)param_find("PWM_MAX");
(void)param_find("PWM_DISARMED");
(void)param_find("PWM_AUX_MIN");
(void)param_find("PWM_AUX_MAX");
(void)param_find("PWM_AUX_DISARMED");
(void)param_find("TRIG_MODE");
(void)param_find("UAVCAN_ENABLE");
(void)param_find("SYS_MC_EST_GROUP");
initialize_parameter_handles(_parameter_handles);
/* fetch initial parameter values */
parameters_update();
@ -641,243 +471,16 @@ Sensors::~Sensors() @@ -641,243 +471,16 @@ Sensors::~Sensors()
int
Sensors::parameters_update()
{
bool rc_valid = true;
float tmpScaleFactor = 0.0f;
float tmpRevFactor = 0.0f;
int ret = PX4_OK;
/* rc values */
for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) {
param_get(_parameter_handles.min[i], &(_parameters.min[i]));
param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
param_get(_parameter_handles.max[i], &(_parameters.max[i]));
param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
tmpRevFactor = tmpScaleFactor * _parameters.rev[i];
/* handle blowup in the scaling factor calculation */
if (!PX4_ISFINITE(tmpScaleFactor) ||
(tmpRevFactor < 0.000001f) ||
(tmpRevFactor > 0.2f)) {
PX4_WARN("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i]));
/* scaling factors do not make sense, lock them down */
_parameters.scaling_factor[i] = 0.0f;
rc_valid = false;
} else {
_parameters.scaling_factor[i] = tmpScaleFactor;
}
}
/* handle wrong values */
if (!rc_valid) {
PX4_ERR("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
}
const char *paramerr = "FAIL PARM LOAD";
/* channel mapping */
if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_rattitude_sw, &(_parameters.rc_map_rattitude_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
int ret = update_parameters(_parameter_handles, _parameters);
if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_kill_sw, &(_parameters.rc_map_kill_sw)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_trans_sw, &(_parameters.rc_map_trans_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_gear_sw, &(_parameters.rc_map_gear_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
PX4_WARN("%s", paramerr);
}
param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2));
param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i]));
if (ret) {
return ret;
}
param_get(_parameter_handles.rc_map_flightmode, &(_parameters.rc_map_flightmode));
param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
_parameters.rc_assist_th = fabs(_parameters.rc_assist_th);
param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th));
_parameters.rc_auto_inv = (_parameters.rc_auto_th < 0);
_parameters.rc_auto_th = fabs(_parameters.rc_auto_th);
param_get(_parameter_handles.rc_rattitude_th, &(_parameters.rc_rattitude_th));
_parameters.rc_rattitude_inv = (_parameters.rc_rattitude_th < 0);
_parameters.rc_rattitude_th = fabs(_parameters.rc_rattitude_th);
param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th));
_parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0);
_parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th);
param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th));
_parameters.rc_return_inv = (_parameters.rc_return_th < 0);
_parameters.rc_return_th = fabs(_parameters.rc_return_th);
param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th));
_parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0);
_parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th);
param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th));
_parameters.rc_acro_inv = (_parameters.rc_acro_th < 0);
_parameters.rc_acro_th = fabs(_parameters.rc_acro_th);
param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th));
_parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0);
_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
param_get(_parameter_handles.rc_killswitch_th, &(_parameters.rc_killswitch_th));
_parameters.rc_killswitch_inv = (_parameters.rc_killswitch_th < 0);
_parameters.rc_killswitch_th = fabs(_parameters.rc_killswitch_th);
param_get(_parameter_handles.rc_trans_th, &(_parameters.rc_trans_th));
_parameters.rc_trans_inv = (_parameters.rc_trans_th < 0);
_parameters.rc_trans_th = fabs(_parameters.rc_trans_th);
param_get(_parameter_handles.rc_gear_th, &(_parameters.rc_gear_th));
_parameters.rc_gear_inv = (_parameters.rc_gear_th < 0);
_parameters.rc_gear_th = fabs(_parameters.rc_gear_th);
_rc_update.update_rc_functions();
/* Airspeed offset */
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
PX4_WARN("%s", paramerr);
} else if (_parameters.battery_voltage_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
_parameters.battery_voltage_scaling = (3.3f / 4096);
param_set(_parameter_handles.battery_voltage_scaling, &_parameters.battery_voltage_scaling);
}
/* scaling of ADC ticks to battery current */
if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
PX4_WARN("%s", paramerr);
} else if (_parameters.battery_current_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
_parameters.battery_current_scaling = (3.3f / 4096);
param_set(_parameter_handles.battery_current_scaling, &_parameters.battery_current_scaling);
}
if (param_get(_parameter_handles.battery_current_offset, &(_parameters.battery_current_offset)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(_parameter_handles.battery_v_div, &(_parameters.battery_v_div)) != OK) {
PX4_WARN("%s", paramerr);
_parameters.battery_v_div = 0.0f;
} else if (_parameters.battery_v_div <= 0.0f) {
/* apply scaling according to defaults if set to default */
#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
_parameters.battery_v_div = 13.653333333f;
#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined ( CONFIG_ARCH_BOARD_MINDPX_V2 )
_parameters.battery_v_div = 10.177939394f;
#elif defined (CONFIG_ARCH_BOARD_AEROCORE)
_parameters.battery_v_div = 7.8196363636f;
#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
_parameters.battery_v_div = 5.7013919372f;
#elif defined (CONFIG_ARCH_BOARD_SITL)
_parameters.battery_v_div = 10.177939394f;
#elif defined (CONFIG_ARCH_BOARD_TAP_V1)
_parameters.battery_v_div = 9.0f;
#else
/* ensure a missing default trips a low voltage lockdown */
_parameters.battery_v_div = 0.0f;
#endif
param_set(_parameter_handles.battery_v_div, &_parameters.battery_v_div);
}
if (param_get(_parameter_handles.battery_a_per_v, &(_parameters.battery_a_per_v)) != OK) {
PX4_WARN("%s", paramerr);
_parameters.battery_a_per_v = 0.0f;
} else if (_parameters.battery_a_per_v <= 0.0f) {
/* apply scaling according to defaults if set to default */
#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
/* current scaling for ACSP4 */
_parameters.battery_a_per_v = 36.367515152f;
#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_MINDPX_V2) || defined (CONFIG_ARCH_BOARD_AEROCORE) || defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
/* current scaling for 3DR power brick */
_parameters.battery_a_per_v = 15.391030303f;
#elif defined (CONFIG_ARCH_BOARD_SITL)
_parameters.battery_a_per_v = 15.391030303f;
#else
/* ensure a missing default leads to an unrealistic current value */
_parameters.battery_a_per_v = 0.0f;
#endif
param_set(_parameter_handles.battery_a_per_v, &_parameters.battery_a_per_v);
}
param_get(_parameter_handles.battery_source, &(_parameters.battery_source));
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
/** fine tune board offset on parameter update **/
/* fine tune board offset */
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1],
@ -886,7 +489,6 @@ Sensors::parameters_update() @@ -886,7 +489,6 @@ Sensors::parameters_update()
_board_rotation = board_rotation_offset * _board_rotation;
/* update barometer qnh setting */
param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
DevHandle h_baro;
DevMgr::getHandle(BARO0_DEVICE_PATH, h_baro);
@ -908,8 +510,6 @@ Sensors::parameters_update() @@ -908,8 +510,6 @@ Sensors::parameters_update()
#endif
param_get(_parameter_handles.vibe_thresh, &_parameters.vibration_warning_threshold);
return ret;
}

5
src/platforms/px4_defines.h
Unescape View File

@ -63,7 +63,10 @@ @@ -63,7 +63,10 @@
/* Define a usable PX4_ISFINITE. Note that PX4_ISFINITE is ONLY used in C++ files,
* therefore, by default, we want to use std::isfinite. */
# define PX4_ISFINITE(x) std::isfinite(x)
#ifdef __cplusplus
#include <cmath>
#define PX4_ISFINITE(x) std::isfinite(x)
#endif
#if defined(__PX4_ROS)
/****************************************************************************

Write Preview
Loading…
Cancel
Save