Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

New naming convention of variables linked to a parameter #11686

Merged
merged 18 commits into from
Apr 3, 2019
Merged
Changes from all commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
1a75635
Parameter update - Add parameter_update.py script used to rename the …
bresch Mar 26, 2019
bbc778a
Parameter update - add -tcpp flag in rg to check C++ files only
bresch Mar 26, 2019
3e1af5c
Parameter update - Rename variables in drivers
bresch Mar 20, 2019
788d7b2
Parameter update - Rename variables in lib
bresch Mar 20, 2019
15a5b5d
Parameter update - Rename variables in templates
bresch Mar 20, 2019
13df179
Parameter update - Rename variables in modules/commander
bresch Mar 20, 2019
519faef
Parameter update - Rename variables in modules/events
bresch Mar 20, 2019
dfa21a4
Parameter update - Rename variables in modules/mc_pos_control
bresch Mar 20, 2019
a31c5aa
Parameter update - Rename variables in modules/fw_pos_control
bresch Mar 25, 2019
c5dbaee
Parameter update - Rename variables in modules/load_mon
bresch Mar 25, 2019
9322931
Parameter update - Rename variables in modules/fw_pos_control
bresch Mar 25, 2019
7023d12
Parameter update - Rename variables in modules/simulator
bresch Mar 25, 2019
613129b
Parameter update - Rename variables in modules/wind_estimator
bresch Mar 25, 2019
b378c50
Parameter update - Rename variables in modules/mc_att_control
bresch Mar 25, 2019
7dea8a1
Parameter update - Rename variables in modules/navigator
bresch Mar 25, 2019
33d2f77
Parameter update - Rename variables in modules/mavlink
bresch Mar 25, 2019
318eb65
Parameter update - Rename variables in modules/ekf2
bresch Mar 26, 2019
8786faa
Add new line at the end of FlightTaskFailsafe.cpp and FlightTaskAutoL…
bresch Mar 26, 2019
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
94 changes: 94 additions & 0 deletions Tools/parameter_update.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,94 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-

############################################################################
#
# Copyright (c) 2018 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: parameter_update.py
Description:
This script searches for declarations of BlockParam, creates a variable name of the
form "_param_name_of_parameter" and replaces the old variable with the new one.

Example:
(ParamExtFloat<px4::params::EKF2_GYR_NOISE>) _gyroNoise,
[...]
_gyroNoise.set(1.f);

is replaced by

(ParamExtFloat<px4::params::EKF2_GYR_NOISE>) _param_ekf2_gyr_noise,
[...]
_param_ekf2_gyr_noise.set(1.f);

Usage:
The script needs a list of files as an input. To quickly find and feed all the file
that it needs to inspect, one can simply pipe the result of a ripgrep -l command as follows:

rg -l '\.[gs]et\(|px4::params::' -tcpp | python parameter_update.py
"""

import re
import fileinput
import sys

filenames = []
for filename in sys.stdin:
filenames.append(filename.rstrip())

pattern = r'^.*<px4::params::([A-Za-z0-9_]+)>\)\s*([A-Za-z0-9_]+)[,\s]?.*$'
regex = re.compile(pattern, re.MULTILINE)

replace_dict = dict()

print("Searching for parameters...")
for filename in filenames:
print(filename)
with open(filename, 'r') as f:
text = f.read()
for match in regex.finditer(text):
replace_dict[match.group(2)] = "_param_" + match.group(1).strip().lower()

print("The following variables will be changed")
for old_var in replace_dict:
print("{} -> {}".format(old_var, replace_dict[old_var]))

for filename in filenames:
print(filename)
for old_var in replace_dict:
for line in fileinput.input(filename, inplace=1):
line = re.sub(r'(>\)\s*)\b' + old_var + r'\b', r'\1' + replace_dict[old_var], line.rstrip()) # replace the declaration
line = re.sub(r'(^\s*)\b' + old_var + r'\b', r'\1' + replace_dict[old_var], line.rstrip()) # replace the 2 liner declaration and when the variable is used a the beginning of a new line
line = re.sub(r'\b' + old_var + r'\b(\.([gs]et|commit)\()',replace_dict[old_var] + r'\1' , line.rstrip()) # replace the usages (with get/set/commit)
line = re.sub(r'(\()\b' + old_var + r'\b', r'\1' + replace_dict[old_var], line.rstrip()) # replace the 2 liner declaration
print(line)
30 changes: 15 additions & 15 deletions src/drivers/heater/heater.cpp
Original file line number Diff line number Diff line change
@@ -156,11 +156,11 @@ void Heater::cycle()
_sensor_temperature = _sensor_accel.temperature;

// Calculate the temperature delta between the setpoint and reported temperature.
float temperature_delta = _p_temperature_setpoint.get() - _sensor_temperature;
float temperature_delta = _param_sens_imu_temp.get() - _sensor_temperature;

// Modulate the heater time on with a feedforward/PI controller.
_proportional_value = temperature_delta * _p_proportional_gain.get();
_integrator_value += temperature_delta * _p_integrator_gain.get();
_proportional_value = temperature_delta * _param_sens_imu_temp_p.get();
_integrator_value += temperature_delta * _param_sens_imu_temp_i.get();

// Constrain the integrator value to no more than 25% of the duty cycle.
_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
@@ -214,7 +214,7 @@ void Heater::initialize_topics()
}

// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
if (_sensor_accel.device_id == (uint32_t)_p_sensor_id.get()) {
if (_sensor_accel.device_id == (uint32_t)_param_sens_temp_id.get()) {
PX4_INFO("IMU sensor identified.");
break;
}
@@ -225,7 +225,7 @@ void Heater::initialize_topics()
PX4_INFO("Device ID: %d", _sensor_accel.device_id);

// Exit the driver if the sensor ID does not match the desired sensor.
if (_sensor_accel.device_id != (uint32_t)_p_sensor_id.get()) {
if (_sensor_accel.device_id != (uint32_t)_param_sens_temp_id.get()) {
request_stop();
PX4_ERR("Could not identify IMU sensor.");
}
@@ -247,11 +247,11 @@ void Heater::initialize_trampoline(void *argv)
float Heater::integrator(char *argv[])
{
if (argv[1]) {
_p_integrator_gain.set(atof(argv[1]));
_param_sens_imu_temp_i.set(atof(argv[1]));
}

PX4_INFO("Integrator gain: %2.5f", (double)_p_integrator_gain.get());
return _p_integrator_gain.get();
PX4_INFO("Integrator gain: %2.5f", (double)_param_sens_imu_temp_i.get());
return _param_sens_imu_temp_i.get();
}

int Heater::orb_update(const struct orb_metadata *meta, int handle, void *buffer)
@@ -278,7 +278,7 @@ int Heater::print_status()
{
PX4_INFO("Temperature: %3.3fC - Setpoint: %3.2fC - Heater State: %s",
(double)_sensor_temperature,
(double)_p_temperature_setpoint.get(),
(double)_param_sens_imu_temp.get(),
_heater_on ? "On" : "Off");

return PX4_OK;
@@ -316,11 +316,11 @@ This task can be started at boot from the startup scripts by setting SENS_EN_THE
float Heater::proportional(char *argv[])
{
if (argv[1]) {
_p_proportional_gain.set(atof(argv[1]));
_param_sens_imu_temp_p.set(atof(argv[1]));
}

PX4_INFO("Proportional gain: %2.5f", (double)_p_proportional_gain.get());
return _p_proportional_gain.get();
PX4_INFO("Proportional gain: %2.5f", (double)_param_sens_imu_temp_p.get());
return _param_sens_imu_temp_p.get();
}

uint32_t Heater::sensor_id()
@@ -368,11 +368,11 @@ int Heater::task_spawn(int argc, char *argv[])
float Heater::temperature_setpoint(char *argv[])
{
if (argv[1]) {
_p_temperature_setpoint.set(atof(argv[1]));
_param_sens_imu_temp.set(atof(argv[1]));
}

PX4_INFO("Target temp: %3.3f", (double)_p_temperature_setpoint.get());
return _p_temperature_setpoint.get();
PX4_INFO("Target temp: %3.3f", (double)_param_sens_imu_temp.get());
return _param_sens_imu_temp.get();
}

void Heater::update_params(const bool force)
8 changes: 4 additions & 4 deletions src/drivers/heater/heater.h
Original file line number Diff line number Diff line change
@@ -209,9 +209,9 @@ class Heater : public ModuleBase<Heater>, public ModuleParams

/** @note Declare local parameters using defined parameters. */
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SENS_IMU_TEMP_I>) _p_integrator_gain,
(ParamFloat<px4::params::SENS_IMU_TEMP_P>) _p_proportional_gain,
(ParamInt<px4::params::SENS_TEMP_ID>) _p_sensor_id,
(ParamFloat<px4::params::SENS_IMU_TEMP>) _p_temperature_setpoint
(ParamFloat<px4::params::SENS_IMU_TEMP_I>) _param_sens_imu_temp_i,
(ParamFloat<px4::params::SENS_IMU_TEMP_P>) _param_sens_imu_temp_p,
(ParamInt<px4::params::SENS_TEMP_ID>) _param_sens_temp_id,
(ParamFloat<px4::params::SENS_IMU_TEMP>) _param_sens_imu_temp
)
};
4 changes: 2 additions & 2 deletions src/drivers/osd/atxxxx/atxxxx.cpp
Original file line number Diff line number Diff line change
@@ -117,7 +117,7 @@ OSDatxxxx::init()
}

// clear the screen
int num_rows = _param_atxxxx_cfg.get() == 1 ? OSD_NUM_ROWS_NTSC : OSD_NUM_ROWS_PAL;
int num_rows = _param_osd_atxxxx_cfg.get() == 1 ? OSD_NUM_ROWS_NTSC : OSD_NUM_ROWS_PAL;

for (int i = 0; i < OSD_CHARS_PER_ROW; i++) {
for (int j = 0; j < num_rows; j++) {
@@ -185,7 +185,7 @@ OSDatxxxx::init_osd()
int ret = PX4_OK;
uint8_t data = OSD_ZERO_BYTE;

if (_param_atxxxx_cfg.get() == 2) {
if (_param_osd_atxxxx_cfg.get() == 2) {
data |= OSD_PAL_TX_MODE;
}

2 changes: 1 addition & 1 deletion src/drivers/osd/atxxxx/atxxxx.h
Original file line number Diff line number Diff line change
@@ -176,6 +176,6 @@ class OSDatxxxx : public device::SPI, public ModuleBase<OSDatxxxx>, public Modul
uint8_t _nav_state{0};

DEFINE_PARAMETERS(
(ParamInt<px4::params::OSD_ATXXXX_CFG>) _param_atxxxx_cfg
(ParamInt<px4::params::OSD_ATXXXX_CFG>) _param_osd_atxxxx_cfg
)
};
4 changes: 2 additions & 2 deletions src/drivers/tap_esc/tap_esc.cpp
Original file line number Diff line number Diff line change
@@ -141,7 +141,7 @@ class TAP_ESC : public cdev::CDev, public ModuleBase<TAP_ESC>, public ModulePara
EscPacket _packet = {};

DEFINE_PARAMETERS(
(ParamInt<px4::params::MC_AIRMODE>) _airmode ///< multicopter air-mode
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode ///< multicopter air-mode
)

void subscribe();
@@ -420,7 +420,7 @@ void TAP_ESC::cycle()
}

if (_mixers) {
_mixers->set_airmode((Mixer::Airmode)_airmode.get());
_mixers->set_airmode((Mixer::Airmode)_param_mc_airmode.get());
}

/* check if anything updated */
6 changes: 4 additions & 2 deletions src/lib/CollisionPrevention/CollisionPrevention.cpp
Original file line number Diff line number Diff line change
@@ -122,8 +122,10 @@ void CollisionPrevention::update_range_constraints()
float angle = math::radians((float)i * obstacle_distance.increment);

//calculate normalized velocity reductions
float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * cos(angle);
float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * sin(angle);
float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - _param_mpc_col_prev_d.get()) * cos(
angle);
float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - _param_mpc_col_prev_d.get()) * sin(
angle);

if (vel_lim_x > 0 && vel_lim_x > _move_constraints_x_normalized(1)) { _move_constraints_x_normalized(1) = vel_lim_x; }

4 changes: 2 additions & 2 deletions src/lib/CollisionPrevention/CollisionPrevention.hpp
Original file line number Diff line number Diff line change
@@ -66,7 +66,7 @@ class CollisionPrevention : public ModuleParams
*/
bool initializeSubscriptions(SubscriptionArray &subscription_array);

bool is_active() { return MPC_COL_PREV_D.get() > 0; }
bool is_active() { return _param_mpc_col_prev_d.get() > 0; }

void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed);

@@ -90,7 +90,7 @@ class CollisionPrevention : public ModuleParams
matrix::Vector2f _move_constraints_y;

DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_COL_PREV_D>) MPC_COL_PREV_D /**< collision prevention keep minimum distance */
(ParamFloat<px4::params::MPC_COL_PREV_D>) _param_mpc_col_prev_d /**< collision prevention keep minimum distance */
)

void update();
10 changes: 5 additions & 5 deletions src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
Original file line number Diff line number Diff line change
@@ -231,7 +231,7 @@ bool FlightTaskAuto::_evaluateTriplets()
_mission_gear = _sub_triplet_setpoint->get().current.landing_gear;
}

if (COM_OBS_AVOID.get() && _sub_vehicle_status->get().is_rotary_wing) {
if (_param_com_obs_avoid.get() && _sub_vehicle_status->get().is_rotary_wing) {
_checkAvoidanceProgress();
}

@@ -243,7 +243,7 @@ void FlightTaskAuto::_set_heading_from_mode()

Vector2f v; // Vector that points towards desired location

switch (MPC_YAW_MODE.get()) {
switch (_param_mpc_yaw_mode.get()) {

case 0: // Heading points towards the current waypoint.
v = Vector2f(_target) - Vector2f(_position);
@@ -337,7 +337,7 @@ void FlightTaskAuto::_checkAvoidanceProgress()

const float pos_to_target_z = fabsf(_triplet_target(2) - _position(2));

if (pos_to_target.length() < _target_acceptance_radius && pos_to_target_z > NAV_MC_ALT_RAD.get()) {
if (pos_to_target.length() < _target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get()) {
// vehicle above or below the target waypoint
pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
}
@@ -410,8 +410,8 @@ void FlightTaskAuto::_setDefaultConstraints()
FlightTask::_setDefaultConstraints();

// only adjust limits if the new limit is lower
if (_constraints.speed_xy >= MPC_XY_CRUISE.get()) {
_constraints.speed_xy = MPC_XY_CRUISE.get();
if (_constraints.speed_xy >= _param_mpc_xy_cruise.get()) {
_constraints.speed_xy = _param_mpc_xy_cruise.get();
}
}

11 changes: 6 additions & 5 deletions src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
Original file line number Diff line number Diff line change
@@ -105,11 +105,12 @@ class FlightTaskAuto : public FlightTask
int _mission_gear = landing_gear_s::GEAR_KEEP;

DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_XY_CRUISE>) MPC_XY_CRUISE,
(ParamFloat<px4::params::MPC_CRUISE_90>) MPC_CRUISE_90, // speed at corner when angle is 90 degrees move to line
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD, //vertical acceptance radius at which waypoints are updated
(ParamInt<px4::params::MPC_YAW_MODE>) MPC_YAW_MODE, // defines how heading is executed,
(ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID // obstacle avoidance active
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_CRUISE_90>) _param_mpc_cruise_90, // speed at corner when angle is 90 degrees move to line
(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
);

private:
12 changes: 6 additions & 6 deletions src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp
Original file line number Diff line number Diff line change
@@ -66,7 +66,7 @@ void FlightTaskAutoLine::_setSpeedAtTarget()
Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+ 1.0f;
_speed_at_target = math::expontialFromLimits(angle, 0.0f, MPC_CRUISE_90.get(), _mc_cruise_speed);
_speed_at_target = math::expontialFromLimits(angle, 0.0f, _param_mpc_cruise_90.get(), _mc_cruise_speed);
}
}

@@ -171,7 +171,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
}

// If yaw offset is large, only accelerate with 0.5 m/s^2.
float acc_max = (fabsf(yaw_diff) > math::radians(MIS_YAW_ERR.get())) ? 0.5f : MPC_ACC_HOR.get();
float acc_max = (fabsf(yaw_diff) > math::radians(_param_mis_yaw_err.get())) ? 0.5f : _param_mpc_acc_hor.get();

if (acc_track > acc_max) {
// accelerate towards target
@@ -210,8 +210,8 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
// limit vertical downwards speed (positive z) close to ground
// for now we use the altitude above home and assume that we want to land at same height as we took off
float vel_limit = math::gradual(_alt_above_ground,
MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
MPC_LAND_SPEED.get(), _constraints.speed_down);
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_mpc_land_speed.get(), _constraints.speed_down);

// Speed at threshold is by default maximum speed. Threshold defines
// the point in z at which vehicle slows down to reach target altitude.
@@ -244,7 +244,7 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
// we want to accelerate

const float acc = (speed_sp - fabsf(_velocity_setpoint(2))) / _deltatime;
const float acc_max = (flying_upward) ? (MPC_ACC_UP_MAX.get() * 0.5f) : (MPC_ACC_DOWN_MAX.get() * 0.5f);
const float acc_max = (flying_upward) ? (_param_mpc_acc_up_max.get() * 0.5f) : (_param_mpc_acc_down_max.get() * 0.5f);

if (acc > acc_max) {
speed_sp = acc_max * _deltatime + fabsf(_velocity_setpoint(2));
@@ -267,4 +267,4 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints()
_velocity_setpoint(2) = 0.0f;
_position_setpoint(2) = _target(2);
}
}
}
8 changes: 4 additions & 4 deletions src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp
Original file line number Diff line number Diff line change
@@ -51,10 +51,10 @@ class FlightTaskAutoLine : public FlightTaskAutoMapper
protected:

DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
(ParamFloat<px4::params::MIS_YAW_ERR>) MIS_YAW_ERR, // yaw-error threshold
(ParamFloat<px4::params::MPC_ACC_HOR>) MPC_ACC_HOR, // acceleration in flight
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) MPC_ACC_UP_MAX,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
);

void _generateSetpoints() override; /**< Generate setpoints along line. */
Loading