mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-04 15:00:04 +08:00
Compare commits
8 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 69d6fc2a7f | |||
| 02f0e35e3f | |||
| feda568e20 | |||
| fe5f2b501b | |||
| fa1ecc60d3 | |||
| 40eafd458b | |||
| fe8dd05624 | |||
| 535dabe00d |
@@ -26,7 +26,6 @@ param set-default NAV_LOITER_RAD 2
|
||||
param set-default CBRK_AIRSPD_CHK 162128
|
||||
|
||||
param set-default GND_MAX_ANG 0.6
|
||||
param set-default GND_WHEEL_BASE 2.0
|
||||
|
||||
param set-default CA_AIRFRAME 5
|
||||
|
||||
|
||||
@@ -26,7 +26,6 @@ param set-default NAV_LOITER_RAD 2
|
||||
param set-default CBRK_AIRSPD_CHK 162128
|
||||
|
||||
param set-default GND_MAX_ANG 0.6
|
||||
param set-default GND_WHEEL_BASE 2.0
|
||||
|
||||
param set-default CA_AIRFRAME 6
|
||||
|
||||
|
||||
@@ -33,7 +33,6 @@ param set-default NAV_LOITER_RAD 2
|
||||
param set-default CBRK_AIRSPD_CHK 162128
|
||||
|
||||
param set-default GND_MAX_ANG 0.6
|
||||
param set-default GND_WHEEL_BASE 3.0
|
||||
|
||||
param set-default CA_AIRFRAME 5
|
||||
|
||||
|
||||
@@ -13,20 +13,19 @@ param set-default GND_SPEED_I 8
|
||||
param set-default GND_SPEED_IMAX 0.125
|
||||
param set-default GND_SPEED_P 2
|
||||
param set-default GND_SPEED_THR_SC 1
|
||||
param set-default GND_SPEED_TRIM 1
|
||||
param set-default GND_SPEED_TRIM 3
|
||||
param set-default GND_THR_CRUISE 0.85
|
||||
param set-default GND_THR_MAX 1
|
||||
param set-default GND_THR_MIN 0
|
||||
|
||||
param set-default MIS_LTRMIN_ALT 0.01
|
||||
param set-default MIS_TAKEOFF_ALT 0.01
|
||||
param set-default NAV_ACC_RAD 0.5
|
||||
param set-default NAV_ACC_RAD 5
|
||||
param set-default NAV_LOITER_RAD 2
|
||||
|
||||
param set-default CBRK_AIRSPD_CHK 162128
|
||||
|
||||
param set-default GND_MAX_ANG 0.6
|
||||
param set-default GND_WHEEL_BASE 2.0
|
||||
|
||||
param set-default CA_AIRFRAME 9
|
||||
|
||||
|
||||
@@ -34,7 +34,6 @@ param set-default GND_THR_MAX 0.5
|
||||
|
||||
# Differential drive acts like ackermann steering with a maximum turn angle of 60 degrees, or pi/3 radians
|
||||
param set-default GND_MAX_ANG 1.042
|
||||
param set-default GND_WHEEL_BASE 0.17
|
||||
|
||||
# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
|
||||
# to support negative throttle.
|
||||
|
||||
@@ -120,7 +120,8 @@ bool is_fixed_wing(const vehicle_status_s ¤t_status)
|
||||
|
||||
bool is_ground_rover(const vehicle_status_s ¤t_status)
|
||||
{
|
||||
return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER;
|
||||
return (current_status.system_type == VEHICLE_TYPE_GROUND_ROVER ||
|
||||
current_status.system_type == VEHICLE_TYPE_BOAT);
|
||||
}
|
||||
|
||||
// End time for currently blinking LED message, 0 if no blink message
|
||||
|
||||
@@ -30,6 +30,7 @@
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__rover_pos_control
|
||||
MAIN rover_pos_control
|
||||
@@ -37,6 +38,7 @@ px4_add_module(
|
||||
RoverPositionControl.cpp
|
||||
RoverPositionControl.hpp
|
||||
DEPENDS
|
||||
RateControl
|
||||
l1
|
||||
pid
|
||||
)
|
||||
|
||||
@@ -102,6 +102,10 @@ void RoverPositionControl::parameters_update(bool force)
|
||||
_param_speed_d.get(),
|
||||
_param_speed_imax.get(),
|
||||
_param_gndspeed_max.get());
|
||||
_rate_control.setGains(matrix::Vector3f(0.0, 0.0, _param_rate_p.get()), matrix::Vector3f(0.0, 0.0, _param_rate_i.get()),
|
||||
matrix::Vector3f(0.0, 0.0, _param_rate_d.get()));
|
||||
_rate_control.setFeedForwardGain(matrix::Vector3f(0.0, 0.0, _param_rate_ff.get()));
|
||||
_rate_control.setIntegratorLimit(matrix::Vector3f(0.0, 0.0, _param_rate_imax.get()));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -151,6 +155,17 @@ RoverPositionControl::manual_control_setpoint_poll()
|
||||
|
||||
_attitude_sp_pub.publish(_att_sp);
|
||||
|
||||
} else if (_control_mode.flag_control_rates_enabled) {
|
||||
// STABILIZED mode generate the attitude setpoint from manual user inputs
|
||||
_rates_sp.roll = 0.0;
|
||||
_rates_sp.pitch = 0.0;
|
||||
_rates_sp.yaw = _manual_control_setpoint.y;
|
||||
_rates_sp.thrust_body[0] = _manual_control_setpoint.z;
|
||||
|
||||
_rates_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
_rates_sp_pub.publish(_rates_sp);
|
||||
|
||||
} else {
|
||||
_act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y;
|
||||
_act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x;
|
||||
@@ -187,6 +202,14 @@ RoverPositionControl::attitude_setpoint_poll()
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
RoverPositionControl::rates_setpoint_poll()
|
||||
{
|
||||
if (_rates_sp_sub.updated()) {
|
||||
_rates_sp_sub.copy(&_rates_sp);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
RoverPositionControl::vehicle_attitude_poll()
|
||||
{
|
||||
@@ -195,6 +218,14 @@ RoverPositionControl::vehicle_attitude_poll()
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
RoverPositionControl::vehicle_angular_acceleration_poll()
|
||||
{
|
||||
if (_vehicle_angular_acceleration_sub.updated()) {
|
||||
_vehicle_angular_acceleration_sub.copy(&_vehicle_angular_acceleration);
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
RoverPositionControl::control_position(const matrix::Vector2d ¤t_position,
|
||||
const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
|
||||
@@ -287,21 +318,30 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position,
|
||||
prev_wp(1));
|
||||
_gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d);
|
||||
|
||||
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
|
||||
///WIP: Convert Lateral acceleration demand to rate control reference
|
||||
float min_speed = _param_gndspeed_min.get();
|
||||
matrix::Vector2f saturated_speed_2d = (ground_speed_2d.norm() < min_speed) ? ground_speed_2d :
|
||||
ground_speed_2d.normalized() * min_speed;
|
||||
|
||||
float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
|
||||
float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());
|
||||
float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
|
||||
_gnd_control.nav_lateral_acceleration_demand());
|
||||
control_effort = math::constrain(control_effort, -1.0f, 1.0f);
|
||||
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
|
||||
float desired_yaw_rate = _gnd_control.nav_lateral_acceleration_demand() / saturated_speed_2d.norm();
|
||||
|
||||
_rates_sp.roll = 0.0;
|
||||
_rates_sp.pitch = 0.0;
|
||||
_rates_sp.yaw = desired_yaw_rate;
|
||||
_rates_sp.thrust_body[0] = math::constrain(mission_throttle, 0.0f, 1.0f);
|
||||
_rates_sp.timestamp = hrt_absolute_time();
|
||||
_rates_sp_pub.publish(_rates_sp);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case STOPPING: {
|
||||
_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
|
||||
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
|
||||
_rates_sp.roll = 0.0;
|
||||
_rates_sp.pitch = 0.0;
|
||||
_rates_sp.yaw = 0.0;
|
||||
_rates_sp.thrust_body[0] = 0.0;
|
||||
_rates_sp.timestamp = hrt_absolute_time();
|
||||
_rates_sp_pub.publish(_rates_sp);
|
||||
// Note _prev_wp is different to the local prev_wp which is related to a mission waypoint.
|
||||
float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0), (double)_prev_wp(1),
|
||||
(double)curr_wp(0), (double)curr_wp(1));
|
||||
@@ -380,31 +420,63 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi
|
||||
// quaternion attitude control law, qe is rotation from q to qd
|
||||
const Quatf qe = Quatf(att.q).inversed() * Quatf(att_sp.q_d);
|
||||
const Eulerf euler_sp = qe;
|
||||
// PX4_INFO("Yaw error: %f", double(euler_sp(2)));
|
||||
|
||||
float control_effort = euler_sp(2) / _param_max_turn_angle.get();
|
||||
control_effort = math::constrain(control_effort, -1.0f, 1.0f);
|
||||
_rates_sp.roll = 0.0;
|
||||
_rates_sp.pitch = 0.0;
|
||||
_rates_sp.yaw = euler_sp(2) / _param_max_turn_angle.get();
|
||||
_rates_sp.thrust_body[0] = math::constrain(att_sp.thrust_body[0], 0.0f, 1.0f);
|
||||
_rates_sp.timestamp = hrt_absolute_time();
|
||||
_rates_sp_pub.publish(_rates_sp);
|
||||
}
|
||||
|
||||
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
|
||||
void
|
||||
RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const vehicle_angular_acceleration_s &acc,
|
||||
const vehicle_local_position_s &local_pos,
|
||||
const vehicle_rates_setpoint_s &rates_sp)
|
||||
{
|
||||
float dt = (_control_rates_last_called > 0) ? hrt_elapsed_time(&_control_rates_last_called) * 1e-6f : 0.01f;
|
||||
_control_rates_last_called = hrt_absolute_time();
|
||||
|
||||
const float control_throttle = att_sp.thrust_body[0];
|
||||
const matrix::Vector3f vehicle_rates(rates.xyz[0], rates.xyz[1], rates.xyz[2]);
|
||||
const matrix::Vector3f rates_setpoint(rates_sp.roll, rates_sp.pitch, rates_sp.yaw);
|
||||
|
||||
const matrix::Vector3f current_velocity(local_pos.vx, local_pos.vy, local_pos.vz);
|
||||
|
||||
bool lock_integrator = false;
|
||||
|
||||
if (current_velocity.norm() < _param_rate_i_minspeed.get()) {
|
||||
_rate_control.resetIntegral();
|
||||
}
|
||||
|
||||
const matrix::Vector3f angular_acceleration{acc.xyz};
|
||||
const matrix::Vector3f desired_angular_acceleration = _rate_control.update(vehicle_rates, rates_setpoint,
|
||||
angular_acceleration, dt,
|
||||
lock_integrator);
|
||||
|
||||
_steering_input = math::constrain(desired_angular_acceleration(2), -1.0f, 1.0f);
|
||||
|
||||
_act_controls.control[actuator_controls_s::INDEX_YAW] = _steering_input;
|
||||
|
||||
const float control_throttle = rates_sp.thrust_body[0];
|
||||
|
||||
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, 1.0f);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
RoverPositionControl::Run()
|
||||
{
|
||||
parameters_update(true);
|
||||
|
||||
/* run controller on gyro changes */
|
||||
vehicle_angular_velocity_s angular_velocity;
|
||||
|
||||
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
|
||||
|
||||
if (_vehicle_angular_velocity_sub.update(&_vehicle_rates)) {
|
||||
// grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy
|
||||
vehicle_angular_acceleration_poll();
|
||||
/* check vehicle control mode for changes to publication state */
|
||||
vehicle_control_mode_poll();
|
||||
attitude_setpoint_poll();
|
||||
rates_setpoint_poll();
|
||||
vehicle_attitude_poll();
|
||||
manual_control_setpoint_poll();
|
||||
|
||||
@@ -413,6 +485,10 @@ RoverPositionControl::Run()
|
||||
/* update parameters from storage */
|
||||
parameters_update();
|
||||
|
||||
if (!PX4_ISFINITE(_steering_input)) {
|
||||
_steering_input = 0.0;
|
||||
}
|
||||
|
||||
/* only run controller if position changed */
|
||||
if (_local_pos_sub.update(&_local_pos)) {
|
||||
|
||||
@@ -492,6 +568,11 @@ RoverPositionControl::Run()
|
||||
|
||||
}
|
||||
|
||||
//Body Rate control
|
||||
if (_control_mode.flag_control_rates_enabled) {
|
||||
control_rates(_vehicle_rates, _vehicle_angular_acceleration, _local_pos, _rates_sp);
|
||||
}
|
||||
|
||||
/* Only publish if any of the proper modes are enabled */
|
||||
if (_control_mode.flag_control_velocity_enabled ||
|
||||
_control_mode.flag_control_attitude_enabled ||
|
||||
|
||||
@@ -41,14 +41,15 @@
|
||||
* @author Marco Zorzi <mzorzi@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <float.h>
|
||||
|
||||
#include <float.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/l1/ECL_L1_Pos_Controller.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/pid/pid.h>
|
||||
#include <lib/rate_control/rate_control.hpp>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
@@ -67,6 +68,9 @@
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
#include <uORB/topics/vehicle_acceleration.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_angular_acceleration.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
@@ -106,6 +110,7 @@ private:
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
|
||||
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */
|
||||
uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */
|
||||
@@ -118,15 +123,20 @@ private:
|
||||
uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
|
||||
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
|
||||
|
||||
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
|
||||
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
|
||||
manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */
|
||||
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
|
||||
vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */
|
||||
vehicle_rates_setpoint_s _rates_sp{}; /**< rate setpoint > */
|
||||
vehicle_control_mode_s _control_mode{}; /**< control mode */
|
||||
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
|
||||
vehicle_local_position_s _local_pos{}; /**< global vehicle position */
|
||||
actuator_controls_s _act_controls{}; /**< direct control of actuators */
|
||||
vehicle_attitude_s _vehicle_att{};
|
||||
vehicle_angular_acceleration_s _vehicle_angular_acceleration{};
|
||||
vehicle_angular_velocity_s _vehicle_rates{};
|
||||
|
||||
trajectory_setpoint_s _trajectory_setpoint{};
|
||||
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
|
||||
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
|
||||
@@ -136,6 +146,7 @@ private:
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
|
||||
hrt_abstime _control_rates_last_called{0};
|
||||
hrt_abstime _manual_setpoint_last_called{0};
|
||||
|
||||
MapProjection _global_local_proj_ref{};
|
||||
@@ -149,6 +160,8 @@ private:
|
||||
uint8_t _pos_reset_counter{0}; // captures the number of times the estimator has reset the horizontal position
|
||||
|
||||
ECL_L1_Pos_Controller _gnd_control;
|
||||
RateControl _rate_control;
|
||||
float _steering_input{0.0};
|
||||
|
||||
enum UGV_POSCTRL_MODE {
|
||||
UGV_POSCTRL_MODE_AUTO,
|
||||
@@ -179,6 +192,7 @@ private:
|
||||
|
||||
(ParamFloat<px4::params::GND_SPEED_TRIM>) _param_gndspeed_trim,
|
||||
(ParamFloat<px4::params::GND_SPEED_MAX>) _param_gndspeed_max,
|
||||
(ParamFloat<px4::params::GND_SPEED_MIN>) _param_gndspeed_min,
|
||||
|
||||
(ParamInt<px4::params::GND_SP_CTRL_MODE>) _param_speed_control_mode,
|
||||
(ParamFloat<px4::params::GND_SPEED_P>) _param_speed_p,
|
||||
@@ -187,12 +201,20 @@ private:
|
||||
(ParamFloat<px4::params::GND_SPEED_IMAX>) _param_speed_imax,
|
||||
(ParamFloat<px4::params::GND_SPEED_THR_SC>) _param_throttle_speed_scaler,
|
||||
|
||||
(ParamFloat<px4::params::GND_RATE_P>) _param_rate_p,
|
||||
(ParamFloat<px4::params::GND_RATE_I>) _param_rate_i,
|
||||
(ParamFloat<px4::params::GND_RATE_D>) _param_rate_d,
|
||||
(ParamFloat<px4::params::GND_RATE_FF>) _param_rate_ff,
|
||||
(ParamFloat<px4::params::GND_RATE_IMAX>) _param_rate_imax,
|
||||
(ParamFloat<px4::params::GND_RATE_MAX>) _param_rate_max,
|
||||
(ParamFloat<px4::params::GND_RATE_IMINSPD>) _param_rate_i_minspeed,
|
||||
|
||||
(ParamFloat<px4::params::GND_THR_MIN>) _param_throttle_min,
|
||||
(ParamFloat<px4::params::GND_THR_MAX>) _param_throttle_max,
|
||||
(ParamFloat<px4::params::GND_THR_CRUISE>) _param_throttle_cruise,
|
||||
|
||||
(ParamFloat<px4::params::GND_WHEEL_BASE>) _param_wheel_base,
|
||||
(ParamFloat<px4::params::GND_MAX_ANG>) _param_max_turn_angle,
|
||||
(ParamFloat<px4::params::GND_ATT_P>) _param_att_p,
|
||||
(ParamFloat<px4::params::GND_MAN_Y_MAX>) _param_gnd_man_y_max,
|
||||
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad /**< loiter radius for Rover */
|
||||
)
|
||||
@@ -204,8 +226,10 @@ private:
|
||||
|
||||
void position_setpoint_triplet_poll();
|
||||
void attitude_setpoint_poll();
|
||||
void rates_setpoint_poll();
|
||||
void vehicle_control_mode_poll();
|
||||
void vehicle_attitude_poll();
|
||||
void vehicle_angular_acceleration_poll();
|
||||
void manual_control_setpoint_poll();
|
||||
|
||||
/**
|
||||
@@ -215,5 +239,8 @@ private:
|
||||
const position_setpoint_triplet_s &_pos_sp_triplet);
|
||||
void control_velocity(const matrix::Vector3f ¤t_velocity);
|
||||
void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);
|
||||
void control_rates(const vehicle_angular_velocity_s &rates, const vehicle_angular_acceleration_s &acc,
|
||||
const vehicle_local_position_s &local_pos,
|
||||
const vehicle_rates_setpoint_s &rates_sp);
|
||||
|
||||
};
|
||||
|
||||
@@ -48,19 +48,6 @@
|
||||
* Controller parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
/**
|
||||
* Distance from front axle to rear axle
|
||||
*
|
||||
* A value of 0.31 is typical for 1/10 RC cars.
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f);
|
||||
|
||||
/**
|
||||
* L1 distance
|
||||
*
|
||||
@@ -247,6 +234,19 @@ PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
|
||||
|
||||
/**
|
||||
* Minimum ground speed
|
||||
*
|
||||
*
|
||||
* @unit m/s
|
||||
* @min 0.0
|
||||
* @max 40
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_SPEED_MIN, 1.0f);
|
||||
|
||||
/**
|
||||
* Maximum ground speed
|
||||
*
|
||||
@@ -275,6 +275,19 @@ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
|
||||
|
||||
/**
|
||||
* Attitude control P gain
|
||||
*
|
||||
*
|
||||
* @unit rad
|
||||
* @min 0.0
|
||||
* @max 5.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_ATT_P, 1.0f);
|
||||
|
||||
/**
|
||||
* Max manual yaw rate
|
||||
*
|
||||
@@ -285,3 +298,87 @@ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f);
|
||||
|
||||
/**
|
||||
* Rover Rate Proportional Gain
|
||||
*
|
||||
* @unit rad/s
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_RATE_P, 5.0f);
|
||||
|
||||
/**
|
||||
* Rover Rate Integral Gain
|
||||
*
|
||||
* @unit rad/s
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_RATE_I, 0.5f);
|
||||
|
||||
/**
|
||||
* Rover Rate Integral Gain
|
||||
*
|
||||
* @unit rad/s
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_RATE_D, 0.0f);
|
||||
|
||||
/**
|
||||
* Rover Rate Proportional Gain
|
||||
*
|
||||
* @unit rad/s
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_RATE_FF, 1.6f);
|
||||
|
||||
/**
|
||||
* Rover Rate Maximum Integral Gain
|
||||
*
|
||||
* @unit rad/s
|
||||
* @min 0.0
|
||||
* @max 50.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_RATE_IMAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Rover Rate Maximum Rate
|
||||
*
|
||||
* @unit rad/s
|
||||
* @min 0.0
|
||||
* @max 50.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_RATE_MAX, 5.0f);
|
||||
|
||||
/**
|
||||
* Rover Rate Integral Minimum speed
|
||||
*
|
||||
* @unit m/s
|
||||
* @min 0.0
|
||||
* @max 50.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_RATE_IMINSPD, 1.0f);
|
||||
|
||||
Reference in New Issue
Block a user