mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-24 03:27:35 +08:00
Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| f23740a3c5 |
@@ -111,7 +111,7 @@ def parse_yaml_parameters_config(yaml_config, ethernet_supported):
|
||||
for key in param['values']:
|
||||
tags += '\n * @value {:} {:}'.format(key, param['values'][key])
|
||||
elif param['type'] == 'boolean':
|
||||
param_type = 'INT32'
|
||||
param_type = 'BOOL'
|
||||
tags += '\n * @boolean'
|
||||
elif param['type'] == 'int32':
|
||||
param_type = 'INT32'
|
||||
@@ -136,9 +136,6 @@ def parse_yaml_parameters_config(yaml_config, ethernet_supported):
|
||||
else:
|
||||
default_value = param['default']
|
||||
|
||||
if type(default_value) == bool:
|
||||
default_value = int(default_value)
|
||||
|
||||
# output the existing C-style format
|
||||
ret += '''
|
||||
/**
|
||||
|
||||
@@ -261,7 +261,7 @@ private:
|
||||
int32_t _val;
|
||||
};
|
||||
|
||||
//external version
|
||||
// external version
|
||||
template<px4::params p>
|
||||
class Param<int32_t &, p>
|
||||
{
|
||||
@@ -318,7 +318,7 @@ class Param<bool, p>
|
||||
{
|
||||
public:
|
||||
// static type-check
|
||||
static_assert(px4::parameters_type[(int)p] == PARAM_TYPE_INT32, "parameter type must be int32_t");
|
||||
static_assert(px4::parameters_type[(int)p] == PARAM_TYPE_BOOL, "parameter type must be bool");
|
||||
|
||||
Param()
|
||||
{
|
||||
@@ -331,18 +331,10 @@ public:
|
||||
const bool &reference() const { return _val; }
|
||||
|
||||
/// Store the parameter value to the parameter storage (@see param_set())
|
||||
bool commit() const
|
||||
{
|
||||
int32_t value_int = (int32_t)_val;
|
||||
return param_set(handle(), &value_int) == 0;
|
||||
}
|
||||
bool commit() const { return param_set(handle(), &_val) == 0; }
|
||||
|
||||
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
|
||||
bool commit_no_notification() const
|
||||
{
|
||||
int32_t value_int = (int32_t)_val;
|
||||
return param_set_no_notification(handle(), &value_int) == 0;
|
||||
}
|
||||
bool commit_no_notification() const { return param_set_no_notification(handle(), &_val) == 0; }
|
||||
|
||||
/// Set and commit a new value. Returns true if the value changed.
|
||||
bool commit_no_notification(bool val)
|
||||
@@ -364,38 +356,83 @@ public:
|
||||
update();
|
||||
}
|
||||
|
||||
bool update()
|
||||
{
|
||||
int32_t value_int;
|
||||
int ret = param_get(handle(), &value_int);
|
||||
|
||||
if (ret == 0) {
|
||||
_val = value_int != 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
bool update() { return param_get(handle(), &_val) == 0; }
|
||||
|
||||
param_t handle() const { return param_handle(p); }
|
||||
private:
|
||||
bool _val;
|
||||
};
|
||||
|
||||
// external version
|
||||
template<px4::params p>
|
||||
class Param<bool &, p>
|
||||
{
|
||||
public:
|
||||
// static type-check
|
||||
static_assert(px4::parameters_type[(int)p] == PARAM_TYPE_BOOL, "parameter type must be bool");
|
||||
|
||||
Param(bool &external_val)
|
||||
: _val(external_val)
|
||||
{
|
||||
param_set_used(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
bool get() const { return _val; }
|
||||
|
||||
const bool &reference() const { return _val; }
|
||||
|
||||
/// Store the parameter value to the parameter storage (@see param_set())
|
||||
bool commit() const { return param_set(handle(), &_val) == 0; }
|
||||
|
||||
/// Store the parameter value to the parameter storage, w/o notifying the system (@see param_set_no_notification())
|
||||
bool commit_no_notification() const { return param_set_no_notification(handle(), &_val) == 0; }
|
||||
|
||||
/// Set and commit a new value. Returns true if the value changed.
|
||||
bool commit_no_notification(bool val)
|
||||
{
|
||||
if (val != _val) {
|
||||
set(val);
|
||||
commit_no_notification();
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void set(bool val) { _val = val; }
|
||||
|
||||
void reset()
|
||||
{
|
||||
param_reset_no_notification(handle());
|
||||
update();
|
||||
}
|
||||
|
||||
bool update() { return param_get(handle(), &_val) == 0; }
|
||||
|
||||
param_t handle() const { return param_handle(p); }
|
||||
private:
|
||||
bool &_val;
|
||||
};
|
||||
|
||||
template <px4::params p>
|
||||
using ParamFloat = Param<float, p>;
|
||||
using ParamBool = Param<bool, p>;
|
||||
|
||||
template <px4::params p>
|
||||
using ParamInt = Param<int32_t, p>;
|
||||
|
||||
template <px4::params p>
|
||||
using ParamExtFloat = Param<float &, p>;
|
||||
using ParamFloat = Param<float, p>;
|
||||
|
||||
|
||||
template <px4::params p>
|
||||
using ParamExtBool = Param<bool &, p>;
|
||||
|
||||
template <px4::params p>
|
||||
using ParamExtInt = Param<int32_t &, p>;
|
||||
|
||||
template <px4::params p>
|
||||
using ParamBool = Param<bool, p>;
|
||||
using ParamExtFloat = Param<float &, p>;
|
||||
|
||||
} /* namespace do_not_explicitly_use_this_namespace */
|
||||
|
||||
|
||||
@@ -39,7 +39,7 @@
|
||||
* @boolean
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_BATT, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_BATT, 0);
|
||||
|
||||
/**
|
||||
* Capacity/current multiplier for high-current capable SMBUS battery
|
||||
|
||||
@@ -70,13 +70,13 @@ CameraCapture::CameraCapture() :
|
||||
|
||||
// get the capture channel from function configuration params
|
||||
param_t p_ctrl_alloc = param_find("SYS_CTRL_ALLOC");
|
||||
int32_t ctrl_alloc = 0;
|
||||
bool ctrl_alloc = false;
|
||||
|
||||
if (p_ctrl_alloc != PARAM_INVALID) {
|
||||
param_get(p_ctrl_alloc, &ctrl_alloc);
|
||||
}
|
||||
|
||||
if (ctrl_alloc == 1) {
|
||||
if (ctrl_alloc) {
|
||||
_capture_channel = -1;
|
||||
|
||||
for (unsigned i = 0; i < 16 && _capture_channel == -1; ++i) {
|
||||
|
||||
@@ -59,7 +59,7 @@ PARAM_DEFINE_FLOAT(CAM_CAP_DELAY, 0.0f);
|
||||
* @group Camera Control
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CAM_CAP_FBACK, 0);
|
||||
PARAM_DEFINE_BOOL(CAM_CAP_FBACK, 0);
|
||||
|
||||
/**
|
||||
* Camera capture timestamping mode
|
||||
@@ -84,4 +84,4 @@ PARAM_DEFINE_INT32(CAM_CAP_MODE, 0);
|
||||
* @group Camera Control
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CAM_CAP_EDGE, 0);
|
||||
PARAM_DEFINE_INT32(CAM_CAP_EDGE, 0);
|
||||
|
||||
@@ -43,14 +43,13 @@ void CameraInterface::get_pins()
|
||||
}
|
||||
|
||||
param_t p_ctrl_alloc = param_find("SYS_CTRL_ALLOC");
|
||||
int32_t ctrl_alloc = 0;
|
||||
bool ctrl_alloc = false;
|
||||
|
||||
if (p_ctrl_alloc != PARAM_INVALID) {
|
||||
param_get(p_ctrl_alloc, &ctrl_alloc);
|
||||
}
|
||||
|
||||
if (ctrl_alloc == 1) {
|
||||
|
||||
if (ctrl_alloc) {
|
||||
unsigned pin_index = 0;
|
||||
|
||||
for (unsigned i = 0; i < 16 && pin_index < arraySize(_pins); ++i) {
|
||||
|
||||
@@ -38,4 +38,4 @@
|
||||
* @group Sensors
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_ETSASPD, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_ETSASPD, 0);
|
||||
|
||||
@@ -38,4 +38,4 @@
|
||||
* @group Sensors
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_MS4525, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_MS4525, 0);
|
||||
|
||||
@@ -37,5 +37,5 @@
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_MS5525, 0);
|
||||
**/
|
||||
PARAM_DEFINE_BOOL(SENS_EN_MS5525, 0);
|
||||
|
||||
@@ -37,5 +37,5 @@
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_SDP3X, 0);
|
||||
**/
|
||||
PARAM_DEFINE_BOOL(SENS_EN_SDP3X, 0);
|
||||
|
||||
@@ -157,7 +157,7 @@ private:
|
||||
perf_counter_t _sample_perf{perf_alloc(PC_COUNT, "mb12xx_sample_perf")};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::SENS_EN_MB12XX>) _p_sensor_enabled,
|
||||
(ParamBool<px4::params::SENS_EN_MB12XX>) _p_sensor_enabled,
|
||||
(ParamInt<px4::params::SENS_MB12_0_ROT>) _p_sensor0_rot,
|
||||
(ParamInt<px4::params::SENS_MB12_1_ROT>) _p_sensor1_rot,
|
||||
(ParamInt<px4::params::SENS_MB12_2_ROT>) _p_sensor2_rot,
|
||||
|
||||
@@ -39,7 +39,7 @@
|
||||
* @boolean
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_MB12XX, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_MB12XX, 0);
|
||||
|
||||
/**
|
||||
* MaxBotix MB12XX Sensor 0 Rotation
|
||||
@@ -291,4 +291,4 @@ PARAM_DEFINE_INT32(SENS_MB12_10_ROT, 0);
|
||||
* @value 6 Yaw 270°
|
||||
* @value 7 Yaw 315°
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_MB12_11_ROT, 0);
|
||||
PARAM_DEFINE_INT32(SENS_MB12_11_ROT, 0);
|
||||
|
||||
@@ -39,4 +39,4 @@
|
||||
* @boolean
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_PGA460, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_PGA460, 0);
|
||||
|
||||
@@ -38,4 +38,4 @@
|
||||
* @group Sensors
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_SR05, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_SR05, 0);
|
||||
|
||||
@@ -39,4 +39,4 @@
|
||||
* @boolean
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_VL53L1X, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_VL53L1X, 0);
|
||||
|
||||
@@ -268,7 +268,7 @@ void LinuxPWMOut::update_params()
|
||||
// PWM_MAIN_REVx
|
||||
{
|
||||
sprintf(str, "%s_REV%u", prefix, i + 1);
|
||||
int32_t pwm_rev = 0;
|
||||
bool pwm_rev = false;
|
||||
|
||||
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
|
||||
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
|
||||
|
||||
@@ -39,4 +39,4 @@
|
||||
* @boolean
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_PAW3902, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_PAW3902, 0);
|
||||
|
||||
@@ -39,4 +39,4 @@
|
||||
* @boolean
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_PMW3901, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_PMW3901, 0);
|
||||
|
||||
@@ -39,4 +39,4 @@
|
||||
* @boolean
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_PX4FLOW, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_PX4FLOW, 0);
|
||||
|
||||
@@ -314,7 +314,7 @@ void PCA9685Wrapper::updatePWMParams()
|
||||
|
||||
if (param_h != PARAM_INVALID) {
|
||||
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
|
||||
int32_t pval = 0;
|
||||
bool pval = false;
|
||||
param_get(param_h, &pval);
|
||||
reverse_pwm_mask &= (0xfffe << i); // clear this bit
|
||||
reverse_pwm_mask |= (((uint16_t)(pval != 0)) << i); // set to new val
|
||||
|
||||
@@ -40,7 +40,7 @@
|
||||
* @boolean
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_INA226, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_INA226, 0);
|
||||
|
||||
/**
|
||||
* INA226 Power Monitor Config
|
||||
|
||||
@@ -40,7 +40,7 @@
|
||||
* @boolean
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_INA228, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_INA228, 0);
|
||||
|
||||
/**
|
||||
* INA228 Power Monitor Config
|
||||
|
||||
@@ -40,7 +40,7 @@
|
||||
* @boolean
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EN_INA238, 0);
|
||||
PARAM_DEFINE_BOOL(SENS_EN_INA238, 0);
|
||||
|
||||
/**
|
||||
* INA238 Power Monitor Max Current
|
||||
|
||||
@@ -655,7 +655,7 @@ void PWMOut::update_params()
|
||||
// PWM_MAIN_REVx
|
||||
{
|
||||
sprintf(str, "%s_REV%u", prefix, i + 1);
|
||||
int32_t pwm_rev = 0;
|
||||
bool pwm_rev = false;
|
||||
|
||||
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
|
||||
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
|
||||
|
||||
@@ -902,7 +902,7 @@ void PX4IO::update_params()
|
||||
|
||||
for (unsigned i = 0; i < _max_actuators; i++) {
|
||||
sprintf(str, "%s_REV%u", prefix, i + 1);
|
||||
int32_t pwm_rev = -1;
|
||||
bool pwm_rev = false;
|
||||
|
||||
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
|
||||
if (pwm_rev >= 1) {
|
||||
|
||||
@@ -40,4 +40,4 @@
|
||||
* @reboot_required true
|
||||
* @group Telemetry
|
||||
*/
|
||||
PARAM_DEFINE_INT32(TEL_BST_EN, 0);
|
||||
PARAM_DEFINE_BOOL(TEL_BST_EN, 0);
|
||||
|
||||
@@ -57,18 +57,18 @@
|
||||
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list)
|
||||
{
|
||||
// airspeed
|
||||
int32_t uavcan_sub_aspd = 1;
|
||||
bool uavcan_sub_aspd = true;
|
||||
param_get(param_find("UAVCAN_SUB_ASPD"), &uavcan_sub_aspd);
|
||||
|
||||
if (uavcan_sub_aspd != 0) {
|
||||
if (uavcan_sub_aspd) {
|
||||
list.add(new UavcanAirspeedBridge(node));
|
||||
}
|
||||
|
||||
// baro
|
||||
int32_t uavcan_sub_baro = 1;
|
||||
bool uavcan_sub_baro = true;
|
||||
param_get(param_find("UAVCAN_SUB_BARO"), &uavcan_sub_baro);
|
||||
|
||||
if (uavcan_sub_baro != 0) {
|
||||
if (uavcan_sub_baro) {
|
||||
list.add(new UavcanBarometerBridge(node));
|
||||
}
|
||||
|
||||
@@ -84,7 +84,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||
}
|
||||
|
||||
// differential pressure
|
||||
int32_t uavcan_sub_dpres = 1;
|
||||
bool uavcan_sub_dpres = true;
|
||||
param_get(param_find("UAVCAN_SUB_DPRES"), &uavcan_sub_dpres);
|
||||
|
||||
if (uavcan_sub_dpres != 0) {
|
||||
@@ -92,7 +92,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||
}
|
||||
|
||||
// flow
|
||||
int32_t uavcan_sub_flow = 1;
|
||||
bool uavcan_sub_flow = true;
|
||||
param_get(param_find("UAVCAN_SUB_FLOW"), &uavcan_sub_flow);
|
||||
|
||||
if (uavcan_sub_flow != 0) {
|
||||
@@ -100,7 +100,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||
}
|
||||
|
||||
// GPS
|
||||
int32_t uavcan_sub_gps = 1;
|
||||
bool uavcan_sub_gps = true;
|
||||
param_get(param_find("UAVCAN_SUB_GPS"), &uavcan_sub_gps);
|
||||
|
||||
if (uavcan_sub_gps != 0) {
|
||||
@@ -108,7 +108,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||
}
|
||||
|
||||
// ice (internal combustion engine)
|
||||
int32_t uavcan_sub_ice = 1;
|
||||
bool uavcan_sub_ice = true;
|
||||
param_get(param_find("UAVCAN_SUB_ICE"), &uavcan_sub_ice);
|
||||
|
||||
if (uavcan_sub_ice != 0) {
|
||||
@@ -116,7 +116,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||
}
|
||||
|
||||
// IMU
|
||||
int32_t uavcan_sub_imu = 1;
|
||||
bool uavcan_sub_imu = true;
|
||||
param_get(param_find("UAVCAN_SUB_IMU"), &uavcan_sub_imu);
|
||||
|
||||
if (uavcan_sub_imu != 0) {
|
||||
@@ -125,7 +125,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||
}
|
||||
|
||||
// magnetometer
|
||||
int32_t uavcan_sub_mag = 1;
|
||||
bool uavcan_sub_mag = true;
|
||||
param_get(param_find("UAVCAN_SUB_MAG"), &uavcan_sub_mag);
|
||||
|
||||
if (uavcan_sub_mag != 0) {
|
||||
@@ -133,7 +133,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge
|
||||
}
|
||||
|
||||
// range finder
|
||||
int32_t uavcan_sub_rng = 1;
|
||||
bool uavcan_sub_rng = true;
|
||||
param_get(param_find("UAVCAN_SUB_RNG"), &uavcan_sub_rng);
|
||||
|
||||
if (uavcan_sub_rng != 0) {
|
||||
|
||||
@@ -206,7 +206,7 @@ PARAM_DEFINE_INT32(UAVCAN_LGT_LAND, 0);
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_SUB_ASPD, 0);
|
||||
PARAM_DEFINE_BOOL(UAVCAN_SUB_ASPD, 0);
|
||||
|
||||
/**
|
||||
* subscription barometer
|
||||
@@ -219,7 +219,7 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_ASPD, 0);
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_SUB_BARO, 0);
|
||||
PARAM_DEFINE_BOOL(UAVCAN_SUB_BARO, 0);
|
||||
|
||||
/**
|
||||
* subscription battery
|
||||
@@ -248,7 +248,7 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_BAT, 0);
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_SUB_DPRES, 0);
|
||||
PARAM_DEFINE_BOOL(UAVCAN_SUB_DPRES, 0);
|
||||
|
||||
/**
|
||||
* subscription flow
|
||||
@@ -259,7 +259,7 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_DPRES, 0);
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_SUB_FLOW, 0);
|
||||
PARAM_DEFINE_BOOL(UAVCAN_SUB_FLOW, 0);
|
||||
|
||||
/**
|
||||
* subscription GPS
|
||||
@@ -273,7 +273,7 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_FLOW, 0);
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_SUB_GPS, 1);
|
||||
PARAM_DEFINE_BOOL(UAVCAN_SUB_GPS, 1);
|
||||
|
||||
/**
|
||||
* subscription ICE
|
||||
@@ -285,7 +285,7 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_GPS, 1);
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_SUB_ICE, 0);
|
||||
PARAM_DEFINE_BOOL(UAVCAN_SUB_ICE, 0);
|
||||
|
||||
/**
|
||||
* subscription IMU
|
||||
@@ -297,7 +297,7 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_ICE, 0);
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_SUB_IMU, 0);
|
||||
PARAM_DEFINE_BOOL(UAVCAN_SUB_IMU, 0);
|
||||
|
||||
/**
|
||||
* subscription magnetometer
|
||||
@@ -310,7 +310,7 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_IMU, 0);
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_SUB_MAG, 1);
|
||||
PARAM_DEFINE_BOOL(UAVCAN_SUB_MAG, 1);
|
||||
|
||||
/**
|
||||
* subscription range finder
|
||||
@@ -322,4 +322,4 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_MAG, 1);
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_SUB_RNG, 0);
|
||||
PARAM_DEFINE_BOOL(UAVCAN_SUB_RNG, 0);
|
||||
|
||||
@@ -178,7 +178,7 @@ private:
|
||||
CanardTransferID _uavcan_node_heartbeat_transfer_id{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
|
||||
(ParamBool<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
|
||||
(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
|
||||
(ParamInt<px4::params::UAVCAN_V1_BAUD>) _param_uavcan_v1_baud
|
||||
)
|
||||
|
||||
@@ -41,7 +41,7 @@
|
||||
* @reboot_required true
|
||||
* @group UAVCAN v1
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_V1_ENABLE, 0);
|
||||
PARAM_DEFINE_BOOL(UAVCAN_V1_ENABLE, 0);
|
||||
|
||||
/**
|
||||
* UAVCAN v1 Node ID.
|
||||
|
||||
@@ -58,7 +58,7 @@ PARAM_DEFINE_INT32(CANNODE_BITRATE, 1000000);
|
||||
* @max 1
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CANNODE_TERM, 0);
|
||||
PARAM_DEFINE_BOOL(CANNODE_TERM, 0);
|
||||
|
||||
/**
|
||||
* Cannode flow board rotation
|
||||
|
||||
@@ -317,7 +317,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData)
|
||||
//at iteration 8 change the CP_GO_NO_DATA to True
|
||||
if (i == 8) {
|
||||
param_t param_allow = param_handle(px4::params::CP_GO_NO_DATA);
|
||||
float value_allow = 1;
|
||||
bool value_allow = true;
|
||||
param_set(param_allow, &value_allow);
|
||||
cp.paramsChanged();
|
||||
}
|
||||
@@ -555,7 +555,7 @@ TEST_F(CollisionPreventionTest, goNoData)
|
||||
|
||||
//WHEN: we change the parameter CP_GO_NO_DATA to allow flying ouside the FOV
|
||||
param_t param_allow = param_handle(px4::params::CP_GO_NO_DATA);
|
||||
float value_allow = 1;
|
||||
bool value_allow = true;
|
||||
param_set(param_allow, &value_allow);
|
||||
cp.paramsChanged();
|
||||
|
||||
|
||||
@@ -83,4 +83,4 @@ PARAM_DEFINE_FLOAT(CP_GUIDE_ANG, 30.f);
|
||||
* @boolean
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CP_GO_NO_DATA, 0);
|
||||
PARAM_DEFINE_BOOL(CP_GO_NO_DATA, 0);
|
||||
|
||||
@@ -331,7 +331,7 @@ private:
|
||||
|
||||
perf_counter_t _control_latency_perf;
|
||||
|
||||
/* SYS_CTRL_ALLOC == 1 */
|
||||
/* SYS_CTRL_ALLOC */
|
||||
FunctionProviderBase *_function_allocated[MAX_ACTUATORS] {}; ///< unique allocated functions
|
||||
FunctionProviderBase *_functions[MAX_ACTUATORS] {}; ///< currently assigned functions
|
||||
OutputFunction _function_assignment[MAX_ACTUATORS] {};
|
||||
|
||||
@@ -94,6 +94,7 @@ param_export_internal(bool only_unsaved, param_filter_func filter)
|
||||
|
||||
while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != nullptr) {
|
||||
|
||||
bool b;
|
||||
int32_t i;
|
||||
float f;
|
||||
|
||||
@@ -114,6 +115,16 @@ param_export_internal(bool only_unsaved, param_filter_func filter)
|
||||
/* append the appropriate BSON type object */
|
||||
|
||||
switch (param_type(s->param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
b = s->val.b;
|
||||
|
||||
if (bson_encoder_append_bool(&encoder, param_name(s->param), b)) {
|
||||
debug("BSON append failed for '%s'", param_name(s->param));
|
||||
goto out;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_INT32:
|
||||
i = s->val.i;
|
||||
|
||||
@@ -205,8 +216,10 @@ struct param_import_state {
|
||||
static int
|
||||
param_import_callback(bson_decoder_t decoder, void *priv, bson_node_t node)
|
||||
{
|
||||
float f;
|
||||
bool b;
|
||||
int32_t i;
|
||||
float f;
|
||||
|
||||
void *v = nullptr;
|
||||
int result = -1;
|
||||
struct param_import_state *state = (struct param_import_state *)priv;
|
||||
@@ -238,6 +251,17 @@ param_import_callback(bson_decoder_t decoder, void *priv, bson_node_t node)
|
||||
*/
|
||||
|
||||
switch (node->type) {
|
||||
case BSON_BOOL:
|
||||
if (param_type(param) != PARAM_TYPE_BOOL) {
|
||||
PX4_WARN("unexpected type for %s", node->name);
|
||||
result = 1; // just skip this entry
|
||||
goto out;
|
||||
}
|
||||
|
||||
b = node->b;
|
||||
v = &b;
|
||||
break;
|
||||
|
||||
case BSON_INT32:
|
||||
if (param_type(param) != PARAM_TYPE_INT32) {
|
||||
PX4_WARN("unexpected type for %s", node->name);
|
||||
|
||||
@@ -57,9 +57,10 @@ __BEGIN_DECLS
|
||||
/**
|
||||
* Parameter types.
|
||||
*/
|
||||
#define PARAM_TYPE_UNKNOWN 0
|
||||
#define PARAM_TYPE_INT32 1
|
||||
#define PARAM_TYPE_FLOAT 2
|
||||
#define PARAM_TYPE_UNKNOWN 0
|
||||
#define PARAM_TYPE_BOOL 1
|
||||
#define PARAM_TYPE_INT32 2
|
||||
#define PARAM_TYPE_FLOAT 3
|
||||
|
||||
typedef uint8_t param_type_t;
|
||||
|
||||
@@ -452,9 +453,10 @@ __EXPORT void param_control_autosave(bool enable);
|
||||
* Parameter value union.
|
||||
*/
|
||||
union param_value_u {
|
||||
void *p;
|
||||
int32_t i;
|
||||
float f;
|
||||
void *p;
|
||||
bool b;
|
||||
int32_t i;
|
||||
float f;
|
||||
};
|
||||
|
||||
/**
|
||||
@@ -493,16 +495,23 @@ __END_DECLS
|
||||
// param is a C-interface. This means there is no overloading, and thus no type-safety for param_get().
|
||||
// So for C++ code we redefine param_get() to inlined overloaded versions, which gives us type-safety
|
||||
// w/o having to use a different interface
|
||||
static inline int param_get_cplusplus(param_t param, float *val)
|
||||
static inline int param_get_cplusplus(param_t param, bool *val)
|
||||
{
|
||||
CHECK_PARAM_TYPE(param, PARAM_TYPE_FLOAT);
|
||||
CHECK_PARAM_TYPE(param, PARAM_TYPE_BOOL);
|
||||
return param_get(param, (void *)val);
|
||||
}
|
||||
|
||||
static inline int param_get_cplusplus(param_t param, int32_t *val)
|
||||
{
|
||||
CHECK_PARAM_TYPE(param, PARAM_TYPE_INT32);
|
||||
return param_get(param, (void *)val);
|
||||
}
|
||||
|
||||
static inline int param_get_cplusplus(param_t param, float *val)
|
||||
{
|
||||
CHECK_PARAM_TYPE(param, PARAM_TYPE_FLOAT);
|
||||
return param_get(param, (void *)val);
|
||||
}
|
||||
#undef CHECK_PARAM_TYPE
|
||||
|
||||
#define param_get(param, val) param_get_cplusplus(param, val)
|
||||
|
||||
@@ -163,5 +163,13 @@ bool param_modify_on_import(bson_node_t node)
|
||||
}
|
||||
}
|
||||
|
||||
// 2021-11-15 migrate INT32 parameters that are now BOOL
|
||||
if ((node->type == BSON_INT32) && (param_type(param_find(node->name)) == PARAM_TYPE_BOOL)) {
|
||||
int32_t old_value = node->i32;
|
||||
|
||||
node->type = BSON_BOOL;
|
||||
node->b = (old_value != 0);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -425,6 +425,9 @@ size_t param_size(param_t param)
|
||||
{
|
||||
if (handle_in_range(param)) {
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
return 1;
|
||||
|
||||
case PARAM_TYPE_INT32:
|
||||
case PARAM_TYPE_FLOAT:
|
||||
return 4;
|
||||
@@ -470,6 +473,9 @@ param_get_value_ptr(param_t param)
|
||||
|
||||
// otherwise return static default value
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
return &px4::parameters[param].val.b;
|
||||
|
||||
case PARAM_TYPE_INT32:
|
||||
return &px4::parameters[param].val.i;
|
||||
|
||||
@@ -537,6 +543,10 @@ param_get_default_value_internal(param_t param, void *default_val)
|
||||
|
||||
// otherwise return static default value
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
memcpy(default_val, &px4::parameters[param].val.b, param_size(param));
|
||||
return PX4_OK;
|
||||
|
||||
case PARAM_TYPE_INT32:
|
||||
memcpy(default_val, &px4::parameters[param].val.i, param_size(param));
|
||||
return PX4_OK;
|
||||
@@ -565,6 +575,25 @@ bool param_value_is_default(param_t param)
|
||||
// back to default, so we don't rely on the params_changed bitset here
|
||||
if (handle_in_range(param)) {
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL: {
|
||||
param_lock_reader();
|
||||
bool default_value = false;
|
||||
|
||||
if (param_get_default_value_internal(param, &default_value) == PX4_OK) {
|
||||
const void *v = param_get_value_ptr(param);
|
||||
|
||||
if (v) {
|
||||
bool current_value;
|
||||
memcpy(¤t_value, v, param_size(param));
|
||||
param_unlock_reader();
|
||||
return (current_value == default_value);
|
||||
}
|
||||
}
|
||||
|
||||
param_unlock_reader();
|
||||
}
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_INT32: {
|
||||
param_lock_reader();
|
||||
int32_t default_value = 0;
|
||||
@@ -618,6 +647,10 @@ param_get_system_default_value(param_t param, void *default_val)
|
||||
int ret = PX4_OK;
|
||||
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
memcpy(default_val, &px4::parameters[param].val.b, param_size(param));
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_INT32:
|
||||
memcpy(default_val, &px4::parameters[param].val.i, param_size(param));
|
||||
break;
|
||||
@@ -771,6 +804,14 @@ param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_
|
||||
if (s != nullptr) {
|
||||
/* update the changed value */
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
param_changed = param_changed || s->val.b != *(bool *)val;
|
||||
s->val.b = *(bool *)val;
|
||||
s->unsaved = !mark_saved;
|
||||
params_changed.set(param, true);
|
||||
result = PX4_OK;
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_INT32:
|
||||
param_changed = param_changed || s->val.i != *(int32_t *)val;
|
||||
s->val.i = *(int32_t *)val;
|
||||
@@ -885,6 +926,10 @@ int param_set_default_value(param_t param, const void *val)
|
||||
bool setting_to_static_default = false;
|
||||
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
setting_to_static_default = (px4::parameters[param].val.b == *(bool *)val);
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_INT32:
|
||||
setting_to_static_default = (px4::parameters[param].val.i == *(int32_t *)val);
|
||||
break;
|
||||
@@ -931,6 +976,12 @@ int param_set_default_value(param_t param, const void *val)
|
||||
if (s != nullptr) {
|
||||
// update the default value
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
s->val.b = *(bool *)val;
|
||||
params_custom_default.set(param, true);
|
||||
result = PX4_OK;
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_INT32:
|
||||
s->val.i = *(int32_t *)val;
|
||||
params_custom_default.set(param, true);
|
||||
@@ -1285,6 +1336,17 @@ param_export(int fd, bool only_unsaved, param_filter_func filter)
|
||||
|
||||
// don't export default values
|
||||
switch (param_type(s->param)) {
|
||||
case PARAM_TYPE_BOOL: {
|
||||
bool default_value = false;
|
||||
param_get_default_value_internal(s->param, &default_value);
|
||||
|
||||
if (s->val.b == default_value) {
|
||||
PX4_DEBUG("skipping %s %d export", param_name(s->param), default_value);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_INT32: {
|
||||
int32_t default_value = 0;
|
||||
param_get_default_value_internal(s->param, &default_value);
|
||||
@@ -1315,6 +1377,17 @@ param_export(int fd, bool only_unsaved, param_filter_func filter)
|
||||
|
||||
/* append the appropriate BSON type object */
|
||||
switch (param_type(s->param)) {
|
||||
case PARAM_TYPE_BOOL: {
|
||||
const bool b = s->val.b;
|
||||
PX4_DEBUG("exporting: %s (%d) size: %lu val: %d", name, s->param, (long unsigned int)size, b);
|
||||
|
||||
if (bson_encoder_append_bool(&encoder, name, b) != 0) {
|
||||
PX4_ERR("BSON append failed for '%s'", name);
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_INT32: {
|
||||
const int32_t i = s->val.i;
|
||||
PX4_DEBUG("exporting: %s (%d) size: %lu val: %d", name, s->param, (long unsigned int)size, i);
|
||||
@@ -1373,8 +1446,10 @@ struct param_import_state {
|
||||
static int
|
||||
param_import_callback(bson_decoder_t decoder, void *priv, bson_node_t node)
|
||||
{
|
||||
float f = 0.0f;
|
||||
bool b = false;
|
||||
int32_t i = 0;
|
||||
float f = 0.0f;
|
||||
|
||||
void *v = nullptr;
|
||||
int result = -1;
|
||||
param_import_state *state = (param_import_state *)priv;
|
||||
@@ -1406,6 +1481,20 @@ param_import_callback(bson_decoder_t decoder, void *priv, bson_node_t node)
|
||||
*/
|
||||
|
||||
switch (node->type) {
|
||||
case BSON_BOOL: {
|
||||
if (param_type(param) != PARAM_TYPE_BOOL) {
|
||||
PX4_WARN("unexpected type for %s", node->name);
|
||||
result = 1; // just skip this entry
|
||||
goto out;
|
||||
}
|
||||
|
||||
b = node->b;
|
||||
v = &b;
|
||||
|
||||
PX4_DEBUG("Imported %s with value %d", param_name(param), b);
|
||||
}
|
||||
break;
|
||||
|
||||
case BSON_INT32: {
|
||||
if (param_type(param) != PARAM_TYPE_INT32) {
|
||||
PX4_WARN("unexpected type for %s", node->name);
|
||||
@@ -1502,9 +1591,10 @@ param_import_internal(int fd, bool mark_saved)
|
||||
} while (result > 0);
|
||||
|
||||
if (result == 0) {
|
||||
PX4_INFO("BSON document size %" PRId32 " bytes, decoded %" PRId32 " bytes (INT32:%" PRIu16 ", FLOAT:%" PRIu16 ")",
|
||||
PX4_INFO("BSON document size %" PRId32 " bytes, decoded %" PRId32 " bytes, bool:%" PRIu16 ", int32:%" PRIu16
|
||||
", double:%" PRIu16,
|
||||
decoder.total_document_size, decoder.total_decoded_size,
|
||||
decoder.count_node_int32, decoder.count_node_double);
|
||||
decoder.count_node_bool, decoder.count_node_int32, decoder.count_node_double);
|
||||
|
||||
return 0;
|
||||
|
||||
|
||||
@@ -25,7 +25,7 @@ class JsonOutput():
|
||||
"reboot_required": ("rebootRequired", bool),
|
||||
"decimal": ("decimalPlaces", int),
|
||||
}
|
||||
allowed_types = { "Uint8", "Int8", "Uint16", "Int16", "Uint32", "Int32", "Float"}
|
||||
allowed_types = { "Bool", "Uint8", "Int8", "Uint16", "Int16", "Uint32", "Int32", "Float"}
|
||||
|
||||
last_param_name = ""
|
||||
board_specific_param_set = False
|
||||
@@ -33,8 +33,9 @@ class JsonOutput():
|
||||
group_name=group.GetName()
|
||||
|
||||
def get_typed_value(value: str, type_name: str):
|
||||
if type_name == 'Float': return float(value)
|
||||
if type_name == 'Bool': return bool(value)
|
||||
if type_name == 'Int32': return int(value)
|
||||
if type_name == 'Float': return float(value)
|
||||
return value
|
||||
|
||||
for param in group.GetParams():
|
||||
|
||||
@@ -381,6 +381,7 @@ class SourceParser(object):
|
||||
seenParamNames.append(name_plus_board)
|
||||
# Validate values
|
||||
default = param.GetDefault()
|
||||
type = param.GetType()
|
||||
min = param.GetFieldValue("min")
|
||||
max = param.GetFieldValue("max")
|
||||
units = param.GetFieldValue("unit")
|
||||
@@ -388,9 +389,11 @@ class SourceParser(object):
|
||||
sys.stderr.write("Invalid unit in {0}: {1}\n".format(name, units))
|
||||
return False
|
||||
#sys.stderr.write("{0} default:{1} min:{2} max:{3}\n".format(name, default, min, max))
|
||||
if default != "" and not self.IsNumber(default):
|
||||
|
||||
if type != "BOOL" and default != "" and not self.IsNumber(default):
|
||||
sys.stderr.write("Default value not number: {0} {1}\n".format(name, default))
|
||||
return False
|
||||
|
||||
# if default != "" and "." not in default:
|
||||
# sys.stderr.write("Default value does not contain dot (e.g. 10 needs to be written as 10.0): {0} {1}\n".format(name, default))
|
||||
# return False
|
||||
|
||||
@@ -24,10 +24,12 @@ static constexpr param_info_s parameters[] = {
|
||||
{% for param in params %}
|
||||
{
|
||||
"{{ param.attrib["name"] }}",
|
||||
{%- if param.attrib["type"] == "FLOAT" %}
|
||||
.val = {{ "{" }} .f = {{ param.attrib["default"] }} {{ "}" }},
|
||||
{%- if param.attrib["type"] == "BOOL" %}
|
||||
.val = {{ "{" }} .b = {{ param.attrib["default"] }}{{ "}" }},
|
||||
{%- elif param.attrib["type"] == "INT32" %}
|
||||
.val = {{ "{" }} .i = {{ param.attrib["default"] }}{{ "}" }},
|
||||
{%- elif param.attrib["type"] == "FLOAT" %}
|
||||
.val = {{ "{" }} .f = {{ param.attrib["default"] }} {{ "}" }},
|
||||
{%- endif %}
|
||||
},
|
||||
{% endfor %}
|
||||
|
||||
@@ -187,7 +187,7 @@ PARAM_DEFINE_INT32(SYS_CAL_TMAX, 10);
|
||||
*
|
||||
* @group System
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SYS_HAS_GPS, 1);
|
||||
PARAM_DEFINE_BOOL(SYS_HAS_GPS, 1);
|
||||
|
||||
/**
|
||||
* Control if the vehicle has a magnetometer
|
||||
@@ -201,7 +201,7 @@ PARAM_DEFINE_INT32(SYS_HAS_GPS, 1);
|
||||
*
|
||||
* @group System
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SYS_HAS_MAG, 1);
|
||||
PARAM_DEFINE_BOOL(SYS_HAS_MAG, 1);
|
||||
|
||||
/**
|
||||
* Control if the vehicle has a barometer
|
||||
@@ -216,7 +216,7 @@ PARAM_DEFINE_INT32(SYS_HAS_MAG, 1);
|
||||
*
|
||||
* @group System
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SYS_HAS_BARO, 1);
|
||||
PARAM_DEFINE_BOOL(SYS_HAS_BARO, 1);
|
||||
|
||||
/**
|
||||
* Enable factory calibration mode
|
||||
@@ -229,7 +229,7 @@ PARAM_DEFINE_INT32(SYS_HAS_BARO, 1);
|
||||
* @boolean
|
||||
* @group System
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SYS_FAC_CAL_MODE, 0);
|
||||
PARAM_DEFINE_BOOL(SYS_FAC_CAL_MODE, 0);
|
||||
|
||||
/**
|
||||
* Bootloader update
|
||||
@@ -251,7 +251,7 @@ PARAM_DEFINE_INT32(SYS_FAC_CAL_MODE, 0);
|
||||
*
|
||||
* @group System
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SYS_BL_UPDATE, 0);
|
||||
PARAM_DEFINE_BOOL(SYS_BL_UPDATE, 0);
|
||||
|
||||
/**
|
||||
* Enable failure injection
|
||||
@@ -266,7 +266,6 @@ PARAM_DEFINE_INT32(SYS_BL_UPDATE, 0);
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SYS_FAILURE_EN, 0);
|
||||
|
||||
|
||||
/**
|
||||
* Enable Dynamic Control Allocation
|
||||
*
|
||||
@@ -280,4 +279,4 @@ PARAM_DEFINE_INT32(SYS_FAILURE_EN, 0);
|
||||
* @reboot_required true
|
||||
* @group System
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SYS_CTRL_ALLOC, 0);
|
||||
PARAM_DEFINE_BOOL(SYS_CTRL_ALLOC, 0);
|
||||
|
||||
@@ -45,7 +45,7 @@
|
||||
* @boolean
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(WV_EN, 0);
|
||||
PARAM_DEFINE_BOOL(WV_EN, 0);
|
||||
|
||||
/**
|
||||
* Weather-vane roll angle to yawrate.
|
||||
@@ -79,4 +79,4 @@ PARAM_DEFINE_FLOAT(WV_ROLL_MIN, 1.0f);
|
||||
* @unit deg/s
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(WV_YRATE_MAX, 90.0f);
|
||||
PARAM_DEFINE_FLOAT(WV_YRATE_MAX, 90.0f);
|
||||
|
||||
@@ -177,7 +177,7 @@ private:
|
||||
(ParamFloat<px4::params::ASPD_SCALE_3>) _param_airspeed_scale_3,
|
||||
(ParamInt<px4::params::ASPD_PRIMARY>) _param_airspeed_primary_index,
|
||||
(ParamInt<px4::params::ASPD_DO_CHECKS>) _param_airspeed_checks_on,
|
||||
(ParamInt<px4::params::ASPD_FALLBACK_GW>) _param_airspeed_fallback_gw,
|
||||
(ParamBool<px4::params::ASPD_FALLBACK_GW>) _param_airspeed_fallback_gw,
|
||||
|
||||
(ParamFloat<px4::params::ASPD_FS_INNOV>) _tas_innov_threshold, /**< innovation check threshold */
|
||||
(ParamFloat<px4::params::ASPD_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
|
||||
|
||||
@@ -169,7 +169,7 @@ PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 7);
|
||||
* @boolean
|
||||
* @group Airspeed Validator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ASPD_FALLBACK_GW, 0);
|
||||
PARAM_DEFINE_BOOL(ASPD_FALLBACK_GW, 0);
|
||||
|
||||
/**
|
||||
* Airspeed failure innovation threshold
|
||||
|
||||
@@ -148,11 +148,11 @@ private:
|
||||
(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg,
|
||||
(ParamFloat<px4::params::ATT_W_GYRO_BIAS>) _param_att_w_gyro_bias,
|
||||
(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl,
|
||||
(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a,
|
||||
(ParamBool<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a,
|
||||
(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m,
|
||||
(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,
|
||||
(ParamBool<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,
|
||||
(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,
|
||||
(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
|
||||
(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
|
||||
)
|
||||
};
|
||||
|
||||
|
||||
@@ -99,7 +99,7 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
|
||||
* @group Attitude Q estimator
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
|
||||
PARAM_DEFINE_BOOL(ATT_MAG_DECL_A, 1);
|
||||
|
||||
/**
|
||||
* External heading usage mode (from Motion capture/Vision)
|
||||
@@ -122,7 +122,7 @@ PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
|
||||
* @group Attitude Q estimator
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ATT_ACC_COMP, 1);
|
||||
PARAM_DEFINE_BOOL(ATT_ACC_COMP, 1);
|
||||
|
||||
/**
|
||||
* Gyro bias limit
|
||||
|
||||
@@ -68,11 +68,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
|
||||
|
||||
/* ---- MAG ---- */
|
||||
{
|
||||
int32_t sys_has_mag = 1;
|
||||
bool sys_has_mag = true;
|
||||
param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
|
||||
|
||||
if (sys_has_mag == 1) {
|
||||
|
||||
if (sys_has_mag) {
|
||||
/* check all sensors individually, but fail only for mandatory ones */
|
||||
for (unsigned i = 0; i < max_optional_mag_count; i++) {
|
||||
const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
|
||||
@@ -142,14 +141,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
|
||||
|
||||
/* ---- BARO ---- */
|
||||
{
|
||||
int32_t sys_has_baro = 1;
|
||||
bool sys_has_baro = true;
|
||||
param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
|
||||
|
||||
bool baro_fail_reported = false;
|
||||
|
||||
/* check all sensors, but fail only for mandatory ones */
|
||||
for (unsigned i = 0; i < max_optional_baro_count; i++) {
|
||||
const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1);
|
||||
const bool required = (i < max_mandatory_baro_count) && sys_has_baro;
|
||||
bool report_fail = (required && report_failures && !baro_fail_reported);
|
||||
|
||||
int32_t device_id = -1;
|
||||
|
||||
@@ -64,10 +64,10 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
|
||||
float mag_test_ratio_limit = 1.f;
|
||||
param_get(param_find("COM_ARM_EKF_YAW"), &mag_test_ratio_limit);
|
||||
|
||||
int32_t arm_without_gps = 0;
|
||||
bool arm_without_gps = 0;
|
||||
param_get(param_find("COM_ARM_WO_GPS"), &arm_without_gps);
|
||||
|
||||
int32_t sys_has_gps = 1;
|
||||
bool sys_has_gps = 1;
|
||||
param_get(param_find("SYS_HAS_GPS"), &sys_has_gps);
|
||||
|
||||
bool gps_success = false;
|
||||
|
||||
@@ -1460,7 +1460,7 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd)
|
||||
return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
|
||||
}
|
||||
|
||||
if (_param_com_mot_test_en.get() != 1) {
|
||||
if (!_param_com_mot_test_en.get()) {
|
||||
return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
|
||||
}
|
||||
|
||||
|
||||
@@ -251,7 +251,7 @@ PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f);
|
||||
* @boolean
|
||||
* @group Commander
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0);
|
||||
PARAM_DEFINE_BOOL(COM_HOME_IN_AIR, 0);
|
||||
|
||||
/**
|
||||
* RC control input mode
|
||||
@@ -323,7 +323,7 @@ PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);
|
||||
* @value 0 Require GPS lock to arm
|
||||
* @value 1 Allow arming without GPS
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);
|
||||
PARAM_DEFINE_BOOL(COM_ARM_WO_GPS, 1);
|
||||
|
||||
/**
|
||||
* Arm switch is a momentary button
|
||||
@@ -335,7 +335,7 @@ PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);
|
||||
* @group Commander
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
|
||||
PARAM_DEFINE_BOOL(COM_ARM_SWISBTN, 0);
|
||||
|
||||
/**
|
||||
* Battery failsafe mode
|
||||
@@ -669,7 +669,7 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 2);
|
||||
* @group Commander
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_REARM_GRACE, 1);
|
||||
PARAM_DEFINE_BOOL(COM_REARM_GRACE, 1);
|
||||
|
||||
/**
|
||||
* Enable RC stick override of auto and/or offboard modes
|
||||
@@ -713,7 +713,7 @@ PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 30.0f);
|
||||
* @group Commander
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);
|
||||
PARAM_DEFINE_BOOL(COM_ARM_MIS_REQ, 0);
|
||||
|
||||
/**
|
||||
* Position control navigation loss response.
|
||||
@@ -736,7 +736,7 @@ PARAM_DEFINE_INT32(COM_POSCTL_NAVL, 0);
|
||||
* @group Commander
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_ARM_AUTH_REQ, 0);
|
||||
PARAM_DEFINE_BOOL(COM_ARM_AUTH_REQ, 0);
|
||||
|
||||
/**
|
||||
* Arm authorizer system id
|
||||
@@ -930,7 +930,7 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
|
||||
* @boolean
|
||||
* @group Commander
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
|
||||
PARAM_DEFINE_BOOL(COM_OBS_AVOID, 0);
|
||||
|
||||
/**
|
||||
* Expect and require a healthy MAVLink parachute system
|
||||
@@ -965,7 +965,7 @@ PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0);
|
||||
* @group Commander
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 0);
|
||||
PARAM_DEFINE_BOOL(COM_ARM_CHK_ESCS, 0);
|
||||
|
||||
/**
|
||||
* Condition to enter prearmed mode
|
||||
@@ -990,7 +990,7 @@ PARAM_DEFINE_INT32(COM_PREARM_MODE, 0);
|
||||
* @boolean
|
||||
* @group Commander
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_MOT_TEST_EN, 1);
|
||||
PARAM_DEFINE_BOOL(COM_MOT_TEST_EN, 1);
|
||||
|
||||
/**
|
||||
* Timeout value for disarming when kill switch is engaged
|
||||
|
||||
@@ -117,7 +117,7 @@ private:
|
||||
(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
|
||||
(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
|
||||
(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
|
||||
(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
|
||||
(ParamBool<px4::params::FD_ESCS_EN>) _param_escs_en,
|
||||
(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
|
||||
)
|
||||
};
|
||||
|
||||
@@ -116,7 +116,7 @@ PARAM_DEFINE_FLOAT(FD_FAIL_P_TTRI, 0.3);
|
||||
* @reboot_required true
|
||||
* @group Failure Detector
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FD_EXT_ATS_EN, 0);
|
||||
PARAM_DEFINE_BOOL(FD_EXT_ATS_EN, 0);
|
||||
|
||||
/**
|
||||
* The PWM threshold from external automatic trigger system for engaging failsafe.
|
||||
@@ -140,7 +140,7 @@ PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);
|
||||
*
|
||||
* @group Failure Detector
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
|
||||
PARAM_DEFINE_BOOL(FD_ESCS_EN, 1);
|
||||
|
||||
/**
|
||||
* Imbalanced propeller check threshold
|
||||
|
||||
@@ -373,8 +373,8 @@ struct parameters {
|
||||
float is_moving_scaler{1.0f}; ///< gain scaler used to adjust the threshold for the on-ground movement detection. Larger values make the test less sensitive.
|
||||
|
||||
// compute synthetic magnetomter Z value if possible
|
||||
int32_t synthesize_mag_z{0};
|
||||
int32_t check_mag_strength{0};
|
||||
bool synthesize_mag_z{0};
|
||||
bool check_mag_strength{0};
|
||||
|
||||
// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
|
||||
float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
|
||||
|
||||
@@ -440,7 +440,7 @@ private:
|
||||
_param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
|
||||
|
||||
// vision estimate fusion
|
||||
(ParamInt<px4::params::EKF2_EV_NOISE_MD>)
|
||||
(ParamBool<px4::params::EKF2_EV_NOISE_MD>)
|
||||
_param_ekf2_ev_noise_md, ///< determine source of vision observation noise
|
||||
(ParamFloat<px4::params::EKF2_EVP_NOISE>)
|
||||
_param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m)
|
||||
@@ -489,7 +489,7 @@ private:
|
||||
// control of airspeed and sideslip fusion
|
||||
(ParamExtFloat<px4::params::EKF2_ARSP_THR>)
|
||||
_param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
|
||||
(ParamInt<px4::params::EKF2_FUSE_BETA>)
|
||||
(ParamBool<px4::params::EKF2_FUSE_BETA>)
|
||||
_param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
|
||||
|
||||
// output predictor filter time constants
|
||||
@@ -542,8 +542,8 @@ private:
|
||||
_param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
|
||||
|
||||
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
|
||||
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check
|
||||
(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>)
|
||||
(ParamExtBool<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check
|
||||
(ParamExtBool<px4::params::EKF2_SYNT_MAG_Z>)
|
||||
_param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone.
|
||||
|
||||
// Used by EKF-GSF experimental yaw estimator
|
||||
|
||||
@@ -717,7 +717,7 @@ PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f);
|
||||
* @boolean
|
||||
* @group EKF2
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0);
|
||||
PARAM_DEFINE_BOOL(EKF2_EV_NOISE_MD, 0);
|
||||
|
||||
/**
|
||||
* Measurement noise for vision position observations used to lower bound or replace the uncertainty included in the message
|
||||
@@ -973,7 +973,7 @@ PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f);
|
||||
* @group EKF2
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_FUSE_BETA, 0);
|
||||
PARAM_DEFINE_BOOL(EKF2_FUSE_BETA, 0);
|
||||
|
||||
/**
|
||||
|
||||
@@ -1358,7 +1358,7 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f);
|
||||
* @group EKF2
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 1);
|
||||
PARAM_DEFINE_BOOL(EKF2_MAG_CHECK, 1);
|
||||
|
||||
/**
|
||||
* Enable synthetic magnetometer Z component measurement.
|
||||
@@ -1372,7 +1372,7 @@ PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 1);
|
||||
* @group EKF2
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_SYNT_MAG_Z, 0);
|
||||
PARAM_DEFINE_BOOL(EKF2_SYNT_MAG_Z, 0);
|
||||
|
||||
/**
|
||||
* Default value of true airspeed used in EKF-GSF AHRS calculation.
|
||||
|
||||
@@ -53,7 +53,7 @@
|
||||
* @boolean
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EV_TSK_STAT_DIS, 0);
|
||||
PARAM_DEFINE_BOOL(EV_TSK_STAT_DIS, 0);
|
||||
|
||||
/**
|
||||
* RC Loss Alarm
|
||||
@@ -68,4 +68,4 @@ PARAM_DEFINE_INT32(EV_TSK_STAT_DIS, 0);
|
||||
* @boolean
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EV_TSK_RC_LOSS, 0);
|
||||
PARAM_DEFINE_BOOL(EV_TSK_RC_LOSS, 0);
|
||||
|
||||
@@ -160,7 +160,7 @@ protected:
|
||||
(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
|
||||
_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
|
||||
(ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
|
||||
(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
|
||||
(ParamBool<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, // obstacle avoidance active
|
||||
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
|
||||
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
|
||||
(ParamBool<px4::params::WV_EN>) _param_wv_en, // enable/disable weather vane (VTOL)
|
||||
|
||||
@@ -159,7 +159,7 @@ private:
|
||||
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
|
||||
(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
|
||||
|
||||
(ParamInt<px4::params::FW_ARSP_SCALE_EN>) _param_fw_arsp_scale_en,
|
||||
(ParamBool<px4::params::FW_ARSP_SCALE_EN>) _param_fw_arsp_scale_en,
|
||||
|
||||
(ParamBool<px4::params::FW_BAT_SCALE_EN>) _param_fw_bat_scale_en,
|
||||
|
||||
|
||||
@@ -293,8 +293,7 @@ PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f);
|
||||
* @boolean
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_W_EN, 0);
|
||||
|
||||
PARAM_DEFINE_BOOL(FW_W_EN, 0);
|
||||
|
||||
/**
|
||||
* Wheel steering rate proportional gain
|
||||
@@ -534,7 +533,7 @@ PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
|
||||
* @boolean
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1);
|
||||
PARAM_DEFINE_BOOL(FW_ARSP_SCALE_EN, 1);
|
||||
|
||||
/**
|
||||
* Manual roll scale
|
||||
@@ -591,7 +590,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f);
|
||||
* @boolean
|
||||
* @group FW Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0);
|
||||
PARAM_DEFINE_BOOL(FW_BAT_SCALE_EN, 0);
|
||||
|
||||
/**
|
||||
* Acro body x max rate.
|
||||
|
||||
@@ -56,7 +56,7 @@
|
||||
* @boolean
|
||||
* @group Autotune
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_AT_START, 0);
|
||||
PARAM_DEFINE_BOOL(FW_AT_START, 0);
|
||||
|
||||
/**
|
||||
* Amplitude of the injected signal
|
||||
|
||||
@@ -420,7 +420,7 @@ private:
|
||||
(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
|
||||
(ParamFloat<px4::params::FW_THR_SLEW_MAX>) _param_fw_thr_slew_max,
|
||||
|
||||
(ParamBool<px4::params::FW_POSCTL_INV_ST>) _param_fw_posctl_inv_st,
|
||||
(ParamInt<px4::params::FW_POSCTL_INV_ST>) _param_fw_posctl_inv_st,
|
||||
|
||||
|
||||
(ParamInt<px4::params::FW_GPSF_LT>) _param_nav_gpsf_lt,
|
||||
|
||||
@@ -342,7 +342,7 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
|
||||
* @boolean
|
||||
* @group FW L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_LND_USETER, 0);
|
||||
PARAM_DEFINE_BOOL(FW_LND_USETER, 0);
|
||||
|
||||
/**
|
||||
* Early landing configuration deployment
|
||||
@@ -358,7 +358,7 @@ PARAM_DEFINE_INT32(FW_LND_USETER, 0);
|
||||
*
|
||||
* @group FW L1 Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(FW_LND_EARLYCFG, 0);
|
||||
PARAM_DEFINE_BOOL(FW_LND_EARLYCFG, 0);
|
||||
|
||||
/**
|
||||
* Flare, minimum pitch
|
||||
|
||||
@@ -50,7 +50,7 @@
|
||||
* @boolean
|
||||
* @group FW Launch detection
|
||||
*/
|
||||
PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
|
||||
PARAM_DEFINE_BOOL(LAUN_ALL_ON, 0);
|
||||
|
||||
/**
|
||||
* Catapult accelerometer threshold.
|
||||
|
||||
@@ -45,7 +45,7 @@
|
||||
* @boolean
|
||||
* @group Runway Takeoff
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RWTO_TKOFF, 0);
|
||||
PARAM_DEFINE_BOOL(RWTO_TKOFF, 0);
|
||||
|
||||
/**
|
||||
* Specifies which heading should be held during runnway takeoff.
|
||||
|
||||
@@ -38,4 +38,4 @@
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(IMU_GYRO_CAL_EN, 1);
|
||||
PARAM_DEFINE_BOOL(IMU_GYRO_CAL_EN, 1);
|
||||
|
||||
@@ -38,7 +38,7 @@
|
||||
* @reboot_required true
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(IMU_GYRO_FFT_EN, 0);
|
||||
PARAM_DEFINE_BOOL(IMU_GYRO_FFT_EN, 0);
|
||||
|
||||
/**
|
||||
* IMU gyro FFT minimum frequency.
|
||||
|
||||
@@ -128,7 +128,7 @@ void MulticopterLandDetector::_update_params()
|
||||
_param_lndmc_z_vel_max.commit_no_notification();
|
||||
}
|
||||
|
||||
int32_t use_hover_thrust_estimate = 0;
|
||||
bool use_hover_thrust_estimate = false;
|
||||
param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate);
|
||||
_params.useHoverThrustEstimate = (use_hover_thrust_estimate == 1);
|
||||
|
||||
|
||||
@@ -37,4 +37,4 @@
|
||||
* @boolean
|
||||
* @group System
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SYS_STCK_EN, 1);
|
||||
PARAM_DEFINE_BOOL(SYS_STCK_EN, 1);
|
||||
|
||||
@@ -193,10 +193,10 @@ void LoggedTopics::add_default_topics()
|
||||
#endif /* CONFIG_ARCH_BOARD_PX4_SITL */
|
||||
|
||||
|
||||
int32_t sys_ctrl_alloc = 0;
|
||||
bool sys_ctrl_alloc = false;
|
||||
param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
|
||||
|
||||
if (sys_ctrl_alloc >= 1) {
|
||||
if (sys_ctrl_alloc) {
|
||||
add_topic("actuator_motors", 100);
|
||||
add_topic("actuator_servos", 100);
|
||||
add_topic("vehicle_angular_acceleration", 20);
|
||||
|
||||
@@ -79,7 +79,7 @@ PARAM_DEFINE_INT32(SDLOG_MODE, 0);
|
||||
* @boolean
|
||||
* @group SD Logging
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SDLOG_BOOT_BAT, 0);
|
||||
PARAM_DEFINE_BOOL(SDLOG_BOOT_BAT, 0);
|
||||
|
||||
/**
|
||||
* Mission Log
|
||||
@@ -172,7 +172,7 @@ PARAM_DEFINE_INT32(SDLOG_DIRS_MAX, 0);
|
||||
* @boolean
|
||||
* @group SD Logging
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SDLOG_UUID, 1);
|
||||
PARAM_DEFINE_BOOL(SDLOG_UUID, 1);
|
||||
|
||||
/**
|
||||
* Logfile Encryption algorithm
|
||||
|
||||
@@ -41,7 +41,7 @@
|
||||
* @reboot_required true
|
||||
* @group Magnetometer Bias Estimator
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MBE_ENABLE, 1);
|
||||
PARAM_DEFINE_BOOL(MBE_ENABLE, 1);
|
||||
|
||||
/**
|
||||
* Mag bias estimator learning gain
|
||||
|
||||
@@ -673,7 +673,7 @@ private:
|
||||
(ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp,
|
||||
(ParamInt<px4::params::MAV_RADIO_TOUT>) _param_mav_radio_timeout,
|
||||
(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl,
|
||||
(ParamBool<px4::params::SYS_FAILURE_EN>) _param_sys_failure_injection_enabled
|
||||
(ParamInt<px4::params::SYS_FAILURE_EN>) _param_sys_failure_injection_enabled
|
||||
)
|
||||
|
||||
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": tx run elapsed")}; /**< loop performance counter */
|
||||
|
||||
@@ -122,14 +122,31 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
|
||||
if (param == PARAM_INVALID) {
|
||||
PX4_ERR("unknown param: %s", name);
|
||||
|
||||
} else if (!((param_type(param) == PARAM_TYPE_INT32 && set.param_type == MAV_PARAM_TYPE_INT32) ||
|
||||
(param_type(param) == PARAM_TYPE_FLOAT && set.param_type == MAV_PARAM_TYPE_REAL32))) {
|
||||
PX4_ERR("param types mismatch param: %s", name);
|
||||
} else if (set.param_type >= MAV_PARAM_TYPE_ENUM_END) {
|
||||
PX4_ERR("invalid param type: %s type: %d", name, set.param_type);
|
||||
|
||||
} else {
|
||||
// According to the mavlink spec we should always acknowledge a write operation.
|
||||
} else if ((param_type(param) == PARAM_TYPE_INT32) && (set.param_type == MAV_PARAM_TYPE_INT32)) {
|
||||
int32_t param_value;
|
||||
memcpy(¶m_value, &set.param_value, sizeof(param_value));
|
||||
param_set(param, &(set.param_value));
|
||||
send_param(param);
|
||||
|
||||
} else if ((param_type(param) == PARAM_TYPE_FLOAT) && (set.param_type == MAV_PARAM_TYPE_REAL32)) {
|
||||
float param_value;
|
||||
memcpy(¶m_value, &set.param_value, sizeof(param_value));
|
||||
param_set(param, &(set.param_value));
|
||||
send_param(param);
|
||||
|
||||
} else if ((param_type(param) == PARAM_TYPE_BOOL) && (set.param_type < MAV_PARAM_TYPE_ENUM_END)) {
|
||||
// a zero value of any valid type is false
|
||||
int32_t param_value;
|
||||
memcpy(¶m_value, &set.param_value, sizeof(param_value));
|
||||
bool param_bool = (param_value != 0);
|
||||
param_set(param, ¶m_bool);
|
||||
send_param(param);
|
||||
|
||||
} else {
|
||||
PX4_ERR("invalid param: %s", name);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -428,14 +445,19 @@ MavlinkParametersManager::send_uavcan()
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
|
||||
if (value.param_type == MAV_PARAM_TYPE_REAL32) {
|
||||
msg.param_type = MAVLINK_TYPE_FLOAT;
|
||||
msg.param_value = value.real_value;
|
||||
if (value.param_type == MAV_PARAM_TYPE_UINT8) {
|
||||
bool val = (bool)value.int_value;
|
||||
memcpy(&msg.param_value, &val, sizeof(bool));
|
||||
msg.param_type = MAVLINK_TYPE_UINT8_T;
|
||||
|
||||
} else {
|
||||
} else if (value.param_type == MAV_PARAM_TYPE_INT32) {
|
||||
int32_t val = (int32_t)value.int_value;
|
||||
memcpy(&msg.param_value, &val, sizeof(int32_t));
|
||||
msg.param_type = MAVLINK_TYPE_INT32_T;
|
||||
|
||||
} else if (value.param_type == MAV_PARAM_TYPE_REAL32) {
|
||||
msg.param_type = MAVLINK_TYPE_FLOAT;
|
||||
msg.param_value = value.real_value;
|
||||
}
|
||||
|
||||
// Re-pack the message with the UAVCAN node ID
|
||||
@@ -516,29 +538,48 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
|
||||
return 1;
|
||||
}
|
||||
|
||||
mavlink_param_value_t msg;
|
||||
mavlink_param_value_t msg{};
|
||||
|
||||
/*
|
||||
* get param value, since MAVLink encodes float and int params in the same
|
||||
* space during transmission, copy param onto float val_buf
|
||||
*/
|
||||
if (param_type(param) == PARAM_TYPE_INT32) {
|
||||
int32_t param_value;
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL: {
|
||||
bool param_value;
|
||||
|
||||
if (param_get(param, ¶m_value) != OK) {
|
||||
return 2;
|
||||
if (param_get(param, ¶m_value) != OK) {
|
||||
return 2;
|
||||
}
|
||||
|
||||
memcpy(&msg.param_value, ¶m_value, param_size(param));
|
||||
}
|
||||
break;
|
||||
|
||||
memcpy(&msg.param_value, ¶m_value, sizeof(param_value));
|
||||
case PARAM_TYPE_INT32: {
|
||||
int32_t param_value;
|
||||
|
||||
} else {
|
||||
float param_value;
|
||||
if (param_get(param, ¶m_value) != OK) {
|
||||
return 2;
|
||||
}
|
||||
|
||||
if (param_get(param, ¶m_value) != OK) {
|
||||
return 2;
|
||||
memcpy(&msg.param_value, ¶m_value, param_size(param));
|
||||
}
|
||||
break;
|
||||
|
||||
msg.param_value = param_value;
|
||||
case PARAM_TYPE_FLOAT: {
|
||||
float param_value;
|
||||
|
||||
if (param_get(param, ¶m_value) != OK) {
|
||||
return 2;
|
||||
}
|
||||
|
||||
memcpy(&msg.param_value, ¶m_value, param_size(param));
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
return 2;
|
||||
}
|
||||
|
||||
msg.param_count = param_count_used();
|
||||
@@ -567,7 +608,10 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
|
||||
* Map onboard parameter type to MAVLink type,
|
||||
* endianess matches (both little endian)
|
||||
*/
|
||||
if (type == PARAM_TYPE_INT32) {
|
||||
if (type == PARAM_TYPE_BOOL) {
|
||||
msg.param_type = MAVLINK_TYPE_UINT8_T;
|
||||
|
||||
} else if (type == PARAM_TYPE_INT32) {
|
||||
msg.param_type = MAVLINK_TYPE_INT32_T;
|
||||
|
||||
} else if (type == PARAM_TYPE_FLOAT) {
|
||||
|
||||
@@ -117,7 +117,7 @@ PARAM_DEFINE_INT32(MAV_TYPE, 2);
|
||||
* @boolean
|
||||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
|
||||
PARAM_DEFINE_BOOL(MAV_USEHILGPS, 0);
|
||||
|
||||
/**
|
||||
* Forward external setpoint messages
|
||||
@@ -128,7 +128,7 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
|
||||
* @boolean
|
||||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
|
||||
PARAM_DEFINE_BOOL(MAV_FWDEXTSP, 1);
|
||||
|
||||
/**
|
||||
* Parameter hash check.
|
||||
@@ -139,7 +139,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
|
||||
* @boolean
|
||||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_HASH_CHK_EN, 1);
|
||||
PARAM_DEFINE_BOOL(MAV_HASH_CHK_EN, 1);
|
||||
|
||||
/**
|
||||
* Heartbeat message forwarding.
|
||||
@@ -150,7 +150,7 @@ PARAM_DEFINE_INT32(MAV_HASH_CHK_EN, 1);
|
||||
* @boolean
|
||||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);
|
||||
PARAM_DEFINE_BOOL(MAV_HB_FORW_EN, 1);
|
||||
|
||||
/**
|
||||
* Activate ODOMETRY loopback.
|
||||
@@ -161,7 +161,7 @@ PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);
|
||||
* @boolean
|
||||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_ODOM_LP, 0);
|
||||
PARAM_DEFINE_BOOL(MAV_ODOM_LP, 0);
|
||||
|
||||
/**
|
||||
* Timeout in seconds for the RADIO_STATUS reports coming in
|
||||
|
||||
@@ -45,7 +45,7 @@
|
||||
* @boolean
|
||||
* @group Autotune
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MC_AT_EN, 0);
|
||||
PARAM_DEFINE_BOOL(MC_AT_EN, 0);
|
||||
|
||||
/**
|
||||
* Start the autotuning sequence
|
||||
@@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(MC_AT_EN, 0);
|
||||
* @boolean
|
||||
* @group Autotune
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MC_AT_START, 0);
|
||||
PARAM_DEFINE_BOOL(MC_AT_START, 0);
|
||||
|
||||
/**
|
||||
* Amplitude of the injected signal
|
||||
|
||||
@@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f);
|
||||
* @boolean
|
||||
* @group Multicopter Position Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MPC_USE_HTE, 1);
|
||||
PARAM_DEFINE_BOOL(MPC_USE_HTE, 1);
|
||||
|
||||
/**
|
||||
* Thrust curve in Manual Mode
|
||||
|
||||
@@ -397,4 +397,4 @@ PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f);
|
||||
* @boolean
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0);
|
||||
PARAM_DEFINE_BOOL(MC_BAT_SCALE_EN, 0);
|
||||
|
||||
@@ -65,7 +65,7 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
|
||||
* @boolean
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MIS_TAKEOFF_REQ, 0);
|
||||
PARAM_DEFINE_BOOL(MIS_TAKEOFF_REQ, 0);
|
||||
|
||||
/**
|
||||
* Minimum Loiter altitude
|
||||
|
||||
@@ -318,7 +318,7 @@ private:
|
||||
_param_nav_fw_altl_rad, /**< acceptance radius for fixedwing altitude before landing*/
|
||||
(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
|
||||
_param_nav_mc_alt_rad, /**< acceptance radius for multicopter altitude */
|
||||
(ParamInt<px4::params::NAV_FORCE_VT>) _param_nav_force_vt, /**< acceptance radius for multicopter altitude */
|
||||
(ParamBool<px4::params::NAV_FORCE_VT>) _param_nav_force_vt, /**< acceptance radius for multicopter altitude */
|
||||
(ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */
|
||||
(ParamFloat<px4::params::NAV_TRAFF_A_RADU>) _param_nav_traff_a_radu, /**< avoidance Distance Unmanned*/
|
||||
(ParamFloat<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/
|
||||
|
||||
@@ -197,4 +197,4 @@ PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);
|
||||
* @boolean
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_FORCE_VT, 1);
|
||||
PARAM_DEFINE_BOOL(NAV_FORCE_VT, 1);
|
||||
|
||||
@@ -208,7 +208,7 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
|
||||
* @category system
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_EXT_I2C_PRB, 1);
|
||||
PARAM_DEFINE_BOOL(SENS_EXT_I2C_PRB, 1);
|
||||
|
||||
/**
|
||||
* Sensors hub IMU mode
|
||||
@@ -232,4 +232,4 @@ PARAM_DEFINE_INT32(SENS_IMU_MODE, 1);
|
||||
* @category system
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_INT_BARO_EN, 1);
|
||||
PARAM_DEFINE_BOOL(SENS_INT_BARO_EN, 1);
|
||||
|
||||
@@ -76,7 +76,7 @@ PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0);
|
||||
* @boolean
|
||||
* @group Sensors
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CAL_MAG_ROT_AUTO, 1);
|
||||
PARAM_DEFINE_BOOL(CAL_MAG_ROT_AUTO, 1);
|
||||
|
||||
/**
|
||||
* Magnetometer max rate.
|
||||
|
||||
@@ -222,7 +222,7 @@ private:
|
||||
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
|
||||
(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
|
||||
(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
|
||||
(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
|
||||
(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
|
||||
)
|
||||
};
|
||||
|
||||
|
||||
@@ -170,7 +170,7 @@ private:
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
|
||||
(ParamBool<px4::params::SENS_MAG_MODE>) _param_sens_mag_mode,
|
||||
(ParamInt<px4::params::SENS_MAG_MODE>) _param_sens_mag_mode,
|
||||
(ParamFloat<px4::params::SENS_MAG_RATE>) _param_sens_mag_rate,
|
||||
(ParamBool<px4::params::SENS_MAG_AUTOCAL>) _param_sens_mag_autocal
|
||||
)
|
||||
|
||||
@@ -183,7 +183,7 @@ private:
|
||||
sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
|
||||
(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
|
||||
)
|
||||
};
|
||||
|
||||
|
||||
@@ -38,4 +38,4 @@
|
||||
* @reboot_required true
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(TC_A_ENABLE, 0);
|
||||
PARAM_DEFINE_BOOL(TC_A_ENABLE, 0);
|
||||
|
||||
@@ -38,4 +38,4 @@
|
||||
* @reboot_required true
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(TC_B_ENABLE, 0);
|
||||
PARAM_DEFINE_BOOL(TC_B_ENABLE, 0);
|
||||
|
||||
@@ -38,4 +38,4 @@
|
||||
* @reboot_required true
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(TC_G_ENABLE, 0);
|
||||
PARAM_DEFINE_BOOL(TC_G_ENABLE, 0);
|
||||
|
||||
@@ -352,7 +352,7 @@ void Standard::fill_actuator_outputs()
|
||||
auto &mc_out = _actuators_out_0->control;
|
||||
auto &fw_out = _actuators_out_1->control;
|
||||
|
||||
const bool elevon_lock = (_params->elevons_mc_lock == 1);
|
||||
const bool elevon_lock = _params->elevons_mc_lock;
|
||||
|
||||
switch (_vtol_schedule.flight_mode) {
|
||||
case vtol_mode::MC_MODE:
|
||||
|
||||
@@ -261,6 +261,7 @@ VtolAttitudeControl::parameters_update()
|
||||
{
|
||||
float v;
|
||||
int32_t l;
|
||||
|
||||
/* idle pwm for mc mode */
|
||||
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
|
||||
|
||||
@@ -269,15 +270,13 @@ VtolAttitudeControl::parameters_update()
|
||||
param_get(_params_handles.fw_motors_off, &_params.fw_motors_off);
|
||||
|
||||
/* vtol fw permanent stabilization */
|
||||
param_get(_params_handles.vtol_fw_permanent_stab, &l);
|
||||
_vtol_vehicle_status.fw_permanent_stab = (l == 1);
|
||||
param_get(_params_handles.vtol_fw_permanent_stab, &_vtol_vehicle_status.fw_permanent_stab);
|
||||
|
||||
param_get(_params_handles.vtol_type, &l);
|
||||
_params.vtol_type = l;
|
||||
|
||||
/* vtol lock elevons in multicopter */
|
||||
param_get(_params_handles.elevons_mc_lock, &l);
|
||||
_params.elevons_mc_lock = (l == 1);
|
||||
param_get(_params_handles.elevons_mc_lock, &_params.elevons_mc_lock);
|
||||
|
||||
/* minimum relative altitude for FW mode (QuadChute) */
|
||||
param_get(_params_handles.fw_min_alt, &v);
|
||||
@@ -351,8 +350,7 @@ VtolAttitudeControl::parameters_update()
|
||||
param_get(_params_handles.dec_to_pitch_ff, &_params.dec_to_pitch_ff);
|
||||
param_get(_params_handles.dec_to_pitch_i, &_params.dec_to_pitch_i);
|
||||
|
||||
param_get(_params_handles.vt_mc_on_fmu, &l);
|
||||
_params.vt_mc_on_fmu = l;
|
||||
param_get(_params_handles.vt_mc_on_fmu, &_params.vt_mc_on_fmu);
|
||||
|
||||
param_get(_params_handles.vt_forward_thrust_enable_mode, &_params.vt_forward_thrust_enable_mode);
|
||||
param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1);
|
||||
|
||||
@@ -60,7 +60,7 @@ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900);
|
||||
* @boolean
|
||||
* @group VTOL Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(VT_FW_PERM_STAB, 0);
|
||||
PARAM_DEFINE_BOOL(VT_FW_PERM_STAB, 0);
|
||||
|
||||
/**
|
||||
* VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2)
|
||||
@@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(VT_TYPE, 0);
|
||||
* @boolean
|
||||
* @group VTOL Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 1);
|
||||
PARAM_DEFINE_BOOL(VT_ELEV_MC_LOCK, 1);
|
||||
|
||||
/**
|
||||
* Duration of a front transition
|
||||
@@ -357,7 +357,7 @@ PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f);
|
||||
* @boolean
|
||||
* @group VTOL Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(VT_MC_ON_FMU, 0);
|
||||
PARAM_DEFINE_BOOL(VT_MC_ON_FMU, 0);
|
||||
|
||||
/**
|
||||
* Minimum pitch angle during hover.
|
||||
|
||||
@@ -612,6 +612,7 @@ static int
|
||||
do_show_quiet(const char *param_name)
|
||||
{
|
||||
param_t param = param_find_no_notification(param_name);
|
||||
bool bb;
|
||||
int32_t ii;
|
||||
float ff;
|
||||
// Print only the param value (can be used in scripts)
|
||||
@@ -621,6 +622,12 @@ do_show_quiet(const char *param_name)
|
||||
}
|
||||
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
if (!param_get(param, &bb)) {
|
||||
PARAM_PRINT("%d", bb);
|
||||
}
|
||||
|
||||
break;
|
||||
case PARAM_TYPE_INT32:
|
||||
if (!param_get(param, &ii)) {
|
||||
PARAM_PRINT("%ld", (long)ii);
|
||||
@@ -662,6 +669,8 @@ do_show_index(const char *index, bool used_index)
|
||||
char *end;
|
||||
int i = strtol(index, &end, 10);
|
||||
param_t param;
|
||||
|
||||
bool bb;
|
||||
int32_t ii;
|
||||
float ff;
|
||||
|
||||
@@ -682,6 +691,12 @@ do_show_index(const char *index, bool used_index)
|
||||
param_name(param), param_get_used_index(param), param_get_index(param));
|
||||
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
if (!param_get(param, &bb)) {
|
||||
PARAM_PRINT("%d\n", bb);
|
||||
}
|
||||
|
||||
break;
|
||||
case PARAM_TYPE_INT32:
|
||||
if (!param_get(param, &ii)) {
|
||||
PARAM_PRINT("%ld\n", (long)ii);
|
||||
@@ -706,8 +721,10 @@ do_show_index(const char *index, bool used_index)
|
||||
static void
|
||||
do_show_print(void *arg, param_t param)
|
||||
{
|
||||
bool b;
|
||||
int32_t i;
|
||||
float f;
|
||||
|
||||
const char *search_string = (const char *)arg;
|
||||
const char *p_name = (const char *)param_name(param);
|
||||
|
||||
@@ -756,6 +773,13 @@ do_show_print(void *arg, param_t param)
|
||||
*/
|
||||
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
if (!param_get(param, &b)) {
|
||||
PARAM_PRINT("%d\n", b);
|
||||
return;
|
||||
}
|
||||
|
||||
break;
|
||||
case PARAM_TYPE_INT32:
|
||||
if (!param_get(param, &i)) {
|
||||
PARAM_PRINT("%ld\n", (long)i);
|
||||
@@ -831,8 +855,10 @@ do_show_print_for_airframe(void *arg, param_t param)
|
||||
static int
|
||||
do_set(const char *name, const char *val, bool fail_on_not_found)
|
||||
{
|
||||
bool b;
|
||||
int32_t i;
|
||||
float f;
|
||||
|
||||
param_t param = param_find(name);
|
||||
|
||||
/* set nothing if parameter cannot be found */
|
||||
@@ -842,11 +868,25 @@ do_set(const char *name, const char *val, bool fail_on_not_found)
|
||||
return (fail_on_not_found) ? 1 : 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* Set parameter if type is known and conversion from string to value turns out fine
|
||||
*/
|
||||
|
||||
// Set parameter if type is known and conversion from string to value turns out fine
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
if (!param_get(param, &b)) {
|
||||
/* convert string */
|
||||
char *end;
|
||||
bool newval = strtol(val, &end, 10);
|
||||
|
||||
if (b != newval) {
|
||||
PARAM_PRINT("%c %s: ",
|
||||
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
|
||||
param_name(param));
|
||||
PARAM_PRINT("curr: %d", b);
|
||||
param_set(param, &newval);
|
||||
PARAM_PRINT(" -> new: %d\n", newval);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
case PARAM_TYPE_INT32:
|
||||
if (!param_get(param, &i)) {
|
||||
|
||||
@@ -911,6 +951,21 @@ do_set_custom_default(const char *name, const char *val, bool silent_fail)
|
||||
|
||||
// Set parameter if type is known and conversion from string to value turns out fine
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL: {
|
||||
bool b;
|
||||
|
||||
if (param_get_default_value(param, &b) == PX4_OK) {
|
||||
/* convert string */
|
||||
char *end;
|
||||
bool newval = strtol(val, &end, 10);
|
||||
|
||||
if ((b != newval) && (param_set_default_value(param, &newval) == PX4_OK)) {
|
||||
PX4_DEBUG(" parameter default: %s %d -> %d", param_name(param), b, newval);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
case PARAM_TYPE_INT32: {
|
||||
int32_t i;
|
||||
|
||||
@@ -958,8 +1013,10 @@ static int
|
||||
do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OPERATOR cmp_op,
|
||||
enum COMPARE_ERROR_LEVEL err_level)
|
||||
{
|
||||
bool b;
|
||||
int32_t i;
|
||||
float f;
|
||||
|
||||
param_t param = param_find(name);
|
||||
|
||||
/* set nothing if parameter cannot be found */
|
||||
@@ -979,6 +1036,33 @@ do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OP
|
||||
int ret = 1;
|
||||
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_BOOL:
|
||||
if (!param_get(param, &b)) {
|
||||
/* convert string */
|
||||
char *end;
|
||||
|
||||
for (unsigned k = 0; k < comparisons; k++) {
|
||||
i = b;
|
||||
int j = strtol(vals[k], &end, 10);
|
||||
|
||||
if (cmp_op == COMPARE_OPERATOR::EQUAL) {
|
||||
if (b && j > 0) {
|
||||
ret = 0;
|
||||
} else if (!b && j == 0) {
|
||||
ret = 0;
|
||||
}
|
||||
} else if (cmp_op == COMPARE_OPERATOR::GREATER) {
|
||||
if (b && j <= 0) {
|
||||
ret = 0;
|
||||
} else if (!b && j < 0) {
|
||||
ret = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_INT32:
|
||||
if (!param_get(param, &i)) {
|
||||
|
||||
|
||||
Reference in New Issue
Block a user