mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 03:33:35 +08:00
Rover: Rewrote gnd_pos_control and removed gnd_att_control (#12239)
This commit is contained in:
Timothy Scott
committed by
Daniel Agar
Daniel Agar
parent
3743e6d8fb
commit
2ed00c9cb6
@@ -0,0 +1,42 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2017 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.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE modules__rover_pos_control
|
||||
MAIN rover_pos_control
|
||||
SRCS
|
||||
RoverPositionControl.cpp
|
||||
DEPENDS
|
||||
git_ecl
|
||||
ecl_l1
|
||||
pid
|
||||
)
|
||||
@@ -0,0 +1,604 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
*
|
||||
* This module is a modification of the fixed wing module and it is designed for ground rovers.
|
||||
* It has been developed starting from the fw module, simplified and improved with dedicated items.
|
||||
*
|
||||
* All the acknowledgments and credits for the fw wing app are reported in those files.
|
||||
*
|
||||
* @author Marco Zorzi <mzorzi@student.ethz.ch>
|
||||
*/
|
||||
|
||||
|
||||
#include "RoverPositionControl.hpp"
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
|
||||
#define ACTUATOR_PUBLISH_PERIOD_MS 4
|
||||
|
||||
static int _control_task = -1; /**< task handle for sensor task */
|
||||
|
||||
using matrix::Eulerf;
|
||||
using matrix::Quatf;
|
||||
using matrix::Vector3f;
|
||||
|
||||
/**
|
||||
* L1 control app start / stop handling function
|
||||
*
|
||||
* @ingroup apps
|
||||
*/
|
||||
extern "C" __EXPORT int rover_pos_control_main(int argc, char *argv[]);
|
||||
|
||||
|
||||
namespace gnd_control
|
||||
{
|
||||
RoverPositionControl *g_control = nullptr;
|
||||
}
|
||||
|
||||
RoverPositionControl::RoverPositionControl() :
|
||||
/* performance counters */
|
||||
_sub_sensors(ORB_ID(sensor_bias)),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "rover position control")) // TODO : do we even need these perf counters
|
||||
{
|
||||
_parameter_handles.l1_period = param_find("GND_L1_PERIOD");
|
||||
_parameter_handles.l1_damping = param_find("GND_L1_DAMPING");
|
||||
_parameter_handles.l1_distance = param_find("GND_L1_DIST");
|
||||
|
||||
_parameter_handles.gndspeed_trim = param_find("GND_SPEED_TRIM");
|
||||
_parameter_handles.gndspeed_max = param_find("GND_SPEED_MAX");
|
||||
|
||||
_parameter_handles.speed_control_mode = param_find("GND_SP_CTRL_MODE");
|
||||
_parameter_handles.speed_p = param_find("GND_SPEED_P");
|
||||
_parameter_handles.speed_i = param_find("GND_SPEED_I");
|
||||
_parameter_handles.speed_d = param_find("GND_SPEED_D");
|
||||
_parameter_handles.speed_imax = param_find("GND_SPEED_IMAX");
|
||||
_parameter_handles.throttle_speed_scaler = param_find("GND_SPEED_THR_SC");
|
||||
|
||||
_parameter_handles.throttle_min = param_find("GND_THR_MIN");
|
||||
_parameter_handles.throttle_max = param_find("GND_THR_MAX");
|
||||
_parameter_handles.throttle_cruise = param_find("GND_THR_CRUISE");
|
||||
|
||||
_parameter_handles.wheel_base = param_find("GND_WHEEL_BASE");
|
||||
_parameter_handles.max_turn_angle = param_find("GND_MAX_ANG");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
}
|
||||
|
||||
RoverPositionControl::~RoverPositionControl()
|
||||
{
|
||||
if (_control_task != -1) {
|
||||
|
||||
/* task wakes up every 100ms or so at the longest */
|
||||
_task_should_exit = true;
|
||||
|
||||
/* wait for a second for the task to quit at our request */
|
||||
unsigned i = 0;
|
||||
|
||||
do {
|
||||
/* wait 20ms */
|
||||
px4_usleep(20000);
|
||||
|
||||
/* if we have given up, kill it */
|
||||
if (++i > 50) {
|
||||
px4_task_delete(_control_task);
|
||||
break;
|
||||
}
|
||||
} while (_control_task != -1);
|
||||
}
|
||||
|
||||
gnd_control::g_control = nullptr;
|
||||
}
|
||||
|
||||
int
|
||||
RoverPositionControl::parameters_update()
|
||||
{
|
||||
/* L1 control parameters */
|
||||
param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping));
|
||||
param_get(_parameter_handles.l1_period, &(_parameters.l1_period));
|
||||
param_get(_parameter_handles.l1_distance, &(_parameters.l1_distance));
|
||||
|
||||
param_get(_parameter_handles.gndspeed_trim, &(_parameters.gndspeed_trim));
|
||||
param_get(_parameter_handles.gndspeed_max, &(_parameters.gndspeed_max));
|
||||
|
||||
param_get(_parameter_handles.speed_control_mode, &(_parameters.speed_control_mode));
|
||||
param_get(_parameter_handles.speed_p, &(_parameters.speed_p));
|
||||
param_get(_parameter_handles.speed_i, &(_parameters.speed_i));
|
||||
param_get(_parameter_handles.speed_d, &(_parameters.speed_d));
|
||||
param_get(_parameter_handles.speed_imax, &(_parameters.speed_imax));
|
||||
param_get(_parameter_handles.throttle_speed_scaler, &(_parameters.throttle_speed_scaler));
|
||||
|
||||
param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
|
||||
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
|
||||
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
|
||||
|
||||
param_get(_parameter_handles.wheel_base, &(_parameters.wheel_base));
|
||||
param_get(_parameter_handles.max_turn_angle, &(_parameters.max_turn_angle));
|
||||
|
||||
_gnd_control.set_l1_damping(_parameters.l1_damping);
|
||||
_gnd_control.set_l1_period(_parameters.l1_period);
|
||||
_gnd_control.set_l1_roll_limit(math::radians(0.0f));
|
||||
|
||||
pid_init(&_speed_ctrl, PID_MODE_DERIVATIV_CALC, 0.01f);
|
||||
pid_set_parameters(&_speed_ctrl,
|
||||
_parameters.speed_p,
|
||||
_parameters.speed_d,
|
||||
_parameters.speed_i,
|
||||
_parameters.speed_imax,
|
||||
_parameters.gndspeed_max);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
RoverPositionControl::vehicle_control_mode_poll()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_control_mode_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
RoverPositionControl::manual_control_setpoint_poll()
|
||||
{
|
||||
bool manual_updated;
|
||||
orb_check(_manual_control_sub, &manual_updated);
|
||||
|
||||
if (manual_updated) {
|
||||
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
RoverPositionControl::position_setpoint_triplet_poll()
|
||||
{
|
||||
bool pos_sp_triplet_updated;
|
||||
orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
|
||||
|
||||
if (pos_sp_triplet_updated) {
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
RoverPositionControl::vehicle_attitude_poll()
|
||||
{
|
||||
bool att_updated;
|
||||
orb_check(_vehicle_attitude_sub, &att_updated);
|
||||
|
||||
if (att_updated) {
|
||||
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_att);
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
RoverPositionControl::control_position(const matrix::Vector2f ¤t_position,
|
||||
const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
|
||||
{
|
||||
float dt = 0.01; // Using non zero value to a avoid division by zero
|
||||
|
||||
if (_control_position_last_called > 0) {
|
||||
dt = hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
|
||||
}
|
||||
|
||||
_control_position_last_called = hrt_absolute_time();
|
||||
|
||||
bool setpoint = true;
|
||||
|
||||
if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) {
|
||||
/* AUTONOMOUS FLIGHT */
|
||||
|
||||
_control_mode_current = UGV_POSCTRL_MODE_AUTO;
|
||||
|
||||
/* get circle mode */
|
||||
//bool was_circle_mode = _gnd_control.circle_mode();
|
||||
|
||||
/* current waypoint (the one currently heading for) */
|
||||
matrix::Vector2f curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
|
||||
|
||||
/* previous waypoint */
|
||||
matrix::Vector2f prev_wp = curr_wp;
|
||||
|
||||
if (pos_sp_triplet.previous.valid) {
|
||||
prev_wp(0) = (float)pos_sp_triplet.previous.lat;
|
||||
prev_wp(1) = (float)pos_sp_triplet.previous.lon;
|
||||
}
|
||||
|
||||
matrix::Vector2f ground_speed_2d(ground_speed);
|
||||
|
||||
float mission_throttle = _parameters.throttle_cruise;
|
||||
|
||||
/* Just control the throttle */
|
||||
if (_parameters.speed_control_mode == 1) {
|
||||
/* control the speed in closed loop */
|
||||
|
||||
float mission_target_speed = _parameters.gndspeed_trim;
|
||||
|
||||
if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) &&
|
||||
_pos_sp_triplet.current.cruising_speed > 0.1f) {
|
||||
mission_target_speed = _pos_sp_triplet.current.cruising_speed;
|
||||
}
|
||||
|
||||
// Velocity in body frame
|
||||
const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed());
|
||||
const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2));
|
||||
|
||||
const float x_vel = vel(0);
|
||||
const float x_acc = _sub_sensors.get().accel_x;
|
||||
|
||||
// Compute airspeed control out and just scale it as a constant
|
||||
mission_throttle = _parameters.throttle_speed_scaler
|
||||
* pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt);
|
||||
|
||||
// Constrain throttle between min and max
|
||||
mission_throttle = math::constrain(mission_throttle, _parameters.throttle_min, _parameters.throttle_max);
|
||||
|
||||
} else {
|
||||
/* Just control throttle in open loop */
|
||||
if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_throttle) &&
|
||||
_pos_sp_triplet.current.cruising_throttle > 0.01f) {
|
||||
|
||||
mission_throttle = _pos_sp_triplet.current.cruising_throttle;
|
||||
}
|
||||
}
|
||||
|
||||
float dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
|
||||
pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
|
||||
|
||||
bool should_idle = true;
|
||||
|
||||
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
|
||||
// Because of noise in measurements, if the rover was always trying to reach an exact point, it would
|
||||
// move around when it should be parked. So, I try to get the rover within loiter_radius/2, but then
|
||||
// once I reach that point, I don't move until I'm outside of loiter_radius.
|
||||
// TODO: Find out if there's a better measurement to use than loiter_radius.
|
||||
if (dist > pos_sp_triplet.current.loiter_radius) {
|
||||
_waypoint_reached = false;
|
||||
|
||||
} else if (dist <= pos_sp_triplet.current.loiter_radius / 2) {
|
||||
_waypoint_reached = true;
|
||||
}
|
||||
|
||||
should_idle = _waypoint_reached;
|
||||
|
||||
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
|
||||
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF ||
|
||||
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
should_idle = false;
|
||||
}
|
||||
|
||||
|
||||
if (should_idle) {
|
||||
_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
|
||||
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
|
||||
|
||||
} else {
|
||||
|
||||
/* waypoint is a plain navigation waypoint or the takeoff waypoint, does not matter */
|
||||
_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
|
||||
|
||||
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
|
||||
|
||||
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, _parameters.wheel_base);
|
||||
float control_effort = (desired_theta / _parameters.max_turn_angle) * math::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;
|
||||
|
||||
}
|
||||
|
||||
} else {
|
||||
_control_mode_current = UGV_POSCTRL_MODE_OTHER;
|
||||
setpoint = false;
|
||||
}
|
||||
|
||||
return setpoint;
|
||||
}
|
||||
|
||||
void
|
||||
RoverPositionControl::task_main()
|
||||
{
|
||||
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||
_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
|
||||
/* rate limit control mode updates to 5Hz */
|
||||
orb_set_interval(_control_mode_sub, 200);
|
||||
|
||||
/* rate limit position updates to 50 Hz */
|
||||
orb_set_interval(_global_pos_sub, 20);
|
||||
|
||||
/* abort on a nonzero return value from the parameter init */
|
||||
if (parameters_update()) {
|
||||
/* parameter setup went wrong, abort */
|
||||
warnx("aborting startup due to errors.");
|
||||
_task_should_exit = true;
|
||||
}
|
||||
|
||||
/* wakeup source(s) */
|
||||
px4_pollfd_struct_t fds[3];
|
||||
|
||||
/* Setup of loop */
|
||||
fds[0].fd = _params_sub;
|
||||
fds[0].events = POLLIN;
|
||||
fds[1].fd = _global_pos_sub;
|
||||
fds[1].events = POLLIN;
|
||||
fds[2].fd = _manual_control_sub;
|
||||
fds[2].events = POLLIN;
|
||||
|
||||
_task_running = true;
|
||||
|
||||
// Absolute time (in us) at which the actuators were last published
|
||||
long actuators_last_published = 0;
|
||||
// Timeout for poll in ms
|
||||
int timeout = 0;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
// The +500 is to round to the nearest millisecond, instead of to the smallest millisecond.
|
||||
timeout = ACTUATOR_PUBLISH_PERIOD_MS - (hrt_absolute_time() - actuators_last_published) / 1000 - 1;
|
||||
timeout = timeout > 0 ? timeout : 0;
|
||||
|
||||
//PX4_INFO("TIMEOUT: %d", timeout);
|
||||
|
||||
/* wait for up to 500ms for data */
|
||||
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), timeout);
|
||||
|
||||
/* this is undesirable but not much we can do - might want to flag unhappy status */
|
||||
if (pret < 0) {
|
||||
warn("poll error %d, %d", pret, errno);
|
||||
continue;
|
||||
}
|
||||
|
||||
/* check vehicle control mode for changes to publication state */
|
||||
vehicle_control_mode_poll();
|
||||
//manual_control_setpoint_poll();
|
||||
|
||||
_sub_sensors.update();
|
||||
|
||||
bool manual_mode = _control_mode.flag_control_manual_enabled;
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
/* read from param to clear updated flag */
|
||||
parameter_update_s update;
|
||||
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
|
||||
|
||||
/* update parameters from storage */
|
||||
parameters_update();
|
||||
}
|
||||
|
||||
/* only run controller if position changed */
|
||||
if (fds[1].revents & POLLIN) {
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
/* load local copies */
|
||||
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
|
||||
|
||||
position_setpoint_triplet_poll();
|
||||
vehicle_attitude_poll();
|
||||
|
||||
// update the reset counters in any case
|
||||
_pos_reset_counter = _global_pos.lat_lon_reset_counter;
|
||||
|
||||
matrix::Vector3f ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
|
||||
matrix::Vector2f current_position((float)_global_pos.lat, (float)_global_pos.lon);
|
||||
|
||||
// This if statement depends upon short-circuiting: If !manual_mode, then control_position(...)
|
||||
// should not be called.
|
||||
// It doesn't really matter if it is called, it will just be bad for performance.
|
||||
if (!manual_mode && control_position(current_position, ground_speed, _pos_sp_triplet)) {
|
||||
|
||||
/* XXX check if radius makes sense here */
|
||||
float turn_distance = _parameters.l1_distance; //_gnd_control.switch_distance(100.0f);
|
||||
|
||||
// publish status
|
||||
position_controller_status_s pos_ctrl_status = {};
|
||||
|
||||
pos_ctrl_status.nav_roll = 0.0f;
|
||||
pos_ctrl_status.nav_pitch = 0.0f;
|
||||
pos_ctrl_status.nav_bearing = _gnd_control.nav_bearing();
|
||||
|
||||
pos_ctrl_status.target_bearing = _gnd_control.target_bearing();
|
||||
pos_ctrl_status.xtrack_error = _gnd_control.crosstrack_error();
|
||||
|
||||
pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
|
||||
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
|
||||
|
||||
pos_ctrl_status.acceptance_radius = turn_distance;
|
||||
pos_ctrl_status.yaw_acceptance = NAN;
|
||||
|
||||
pos_ctrl_status.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_pos_ctrl_status_pub != nullptr) {
|
||||
orb_publish(ORB_ID(position_controller_status), _pos_ctrl_status_pub, &pos_ctrl_status);
|
||||
|
||||
} else {
|
||||
_pos_ctrl_status_pub = orb_advertise(ORB_ID(position_controller_status), &pos_ctrl_status);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
if (fds[2].revents & POLLIN) {
|
||||
|
||||
// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
|
||||
// returning immediately and this loop will eat up resources.
|
||||
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
|
||||
|
||||
if (manual_mode) {
|
||||
/* manual/direct control */
|
||||
//PX4_INFO("Manual mode!");
|
||||
_act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual.y;
|
||||
_act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual.x;
|
||||
_act_controls.control[actuator_controls_s::INDEX_YAW] = _manual.r; //TODO: Readd yaw scale param
|
||||
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
|
||||
}
|
||||
}
|
||||
|
||||
if (pret == 0) {
|
||||
|
||||
//orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_att);
|
||||
_act_controls.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_actuator_controls_pub != nullptr) {
|
||||
//PX4_INFO("Publishing actuator from pos control");
|
||||
orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &_act_controls);
|
||||
|
||||
} else {
|
||||
|
||||
_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &_act_controls);
|
||||
}
|
||||
|
||||
actuators_last_published = hrt_absolute_time();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
orb_unsubscribe(_control_mode_sub);
|
||||
orb_unsubscribe(_global_pos_sub);
|
||||
orb_unsubscribe(_manual_control_sub);
|
||||
orb_unsubscribe(_params_sub);
|
||||
orb_unsubscribe(_pos_sp_triplet_sub);
|
||||
orb_unsubscribe(_vehicle_attitude_sub);
|
||||
|
||||
_task_running = false;
|
||||
|
||||
warnx("exiting.\n");
|
||||
|
||||
_control_task = -1;
|
||||
}
|
||||
|
||||
int
|
||||
RoverPositionControl::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
gnd_control::g_control = new RoverPositionControl();
|
||||
|
||||
if (gnd_control::g_control == nullptr) {
|
||||
warnx("OUT OF MEM");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* only returns on exit */
|
||||
gnd_control::g_control->task_main();
|
||||
delete gnd_control::g_control;
|
||||
gnd_control::g_control = nullptr;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
RoverPositionControl::start()
|
||||
{
|
||||
/* start the task */
|
||||
_control_task = px4_task_spawn_cmd("rover_pos_ctrl",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_POSITION_CONTROL,
|
||||
1700,
|
||||
(px4_main_t)&RoverPositionControl::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
if (_control_task < 0) {
|
||||
warn("task start failed");
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int rover_pos_control_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
warnx("usage: rover_pos_control {start|stop|status}");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (gnd_control::g_control != nullptr) {
|
||||
warnx("already running");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (OK != RoverPositionControl::start()) {
|
||||
warn("start failed");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
|
||||
while (gnd_control::g_control == nullptr || !gnd_control::g_control->task_running()) {
|
||||
px4_usleep(50000);
|
||||
printf(".");
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
if (gnd_control::g_control == nullptr) {
|
||||
warnx("not running");
|
||||
return 1;
|
||||
}
|
||||
|
||||
delete gnd_control::g_control;
|
||||
gnd_control::g_control = nullptr;
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (gnd_control::g_control) {
|
||||
warnx("running");
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
warnx("not running");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
warnx("unrecognized command");
|
||||
return 1;
|
||||
}
|
||||
@@ -0,0 +1,216 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
*
|
||||
* This module is a modification of the fixed wing module and it is designed for ground rovers.
|
||||
* It has been developed starting from the fw module, simplified and improved with dedicated items.
|
||||
*
|
||||
* All the ackowledgments and credits for the fw wing app are reported in those files.
|
||||
*
|
||||
* @author Marco Zorzi <mzorzi@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <float.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/ecl/l1/ecl_l1_pos_controller.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/pid/pid.h>
|
||||
#include <px4_config.h>
|
||||
#include <px4_defines.h>
|
||||
#include <px4_posix.h>
|
||||
#include <px4_tasks.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/position_controller_status.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/sensor_bias.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/ekf2_timestamps.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
using matrix::Dcmf;
|
||||
|
||||
using uORB::SubscriptionData;
|
||||
|
||||
class RoverPositionControl
|
||||
{
|
||||
public:
|
||||
RoverPositionControl();
|
||||
~RoverPositionControl();
|
||||
RoverPositionControl(const RoverPositionControl &) = delete;
|
||||
RoverPositionControl operator=(const RoverPositionControl &other) = delete;
|
||||
|
||||
/**
|
||||
* Start the sensors task.
|
||||
*
|
||||
* @return OK on success.
|
||||
*/
|
||||
static int start();
|
||||
|
||||
/**
|
||||
* Task status
|
||||
*
|
||||
* @return true if the mainloop is running
|
||||
*/
|
||||
bool task_running() { return _task_running; }
|
||||
|
||||
private:
|
||||
orb_advert_t _pos_ctrl_status_pub{nullptr}; /**< navigation capabilities publication */
|
||||
orb_advert_t _actuator_controls_pub{nullptr}; /**< actuator controls publication */
|
||||
|
||||
bool _task_should_exit{false}; /**< if true, sensor task should exit */
|
||||
bool _task_running{false}; /**< if true, task is running in its mainloop */
|
||||
|
||||
int _control_mode_sub{-1}; /**< control mode subscription */
|
||||
int _global_pos_sub{-1};
|
||||
int _manual_control_sub{-1}; /**< notification of manual control updates */
|
||||
int _params_sub{-1}; /**< notification of parameter updates */
|
||||
int _pos_sp_triplet_sub{-1};
|
||||
int _vehicle_attitude_sub{-1};
|
||||
|
||||
manual_control_setpoint_s _manual{}; /**< r/c channel data */
|
||||
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
|
||||
vehicle_control_mode_s _control_mode{}; /**< control mode */
|
||||
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
|
||||
actuator_controls_s _act_controls{}; /**< direct control of actuators */
|
||||
vehicle_attitude_s _vehicle_att{};
|
||||
|
||||
SubscriptionData<sensor_bias_s> _sub_sensors;
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
|
||||
|
||||
/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
|
||||
the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
|
||||
PID_t _speed_ctrl{};
|
||||
|
||||
// estimator reset counters
|
||||
uint8_t _pos_reset_counter{0}; // captures the number of times the estimator has reset the horizontal position
|
||||
|
||||
ECL_L1_Pos_Controller _gnd_control;
|
||||
|
||||
bool _waypoint_reached;
|
||||
|
||||
enum UGV_POSCTRL_MODE {
|
||||
UGV_POSCTRL_MODE_AUTO,
|
||||
UGV_POSCTRL_MODE_OTHER
|
||||
} _control_mode_current{UGV_POSCTRL_MODE_OTHER}; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
|
||||
|
||||
struct {
|
||||
float l1_period;
|
||||
float l1_damping;
|
||||
float l1_distance;
|
||||
|
||||
float gndspeed_trim;
|
||||
float gndspeed_max;
|
||||
|
||||
int32_t speed_control_mode;
|
||||
float speed_p;
|
||||
float speed_i;
|
||||
float speed_d;
|
||||
float speed_imax;
|
||||
float throttle_speed_scaler;
|
||||
|
||||
float throttle_min;
|
||||
float throttle_max;
|
||||
float throttle_cruise;
|
||||
float throttle_slew_max;
|
||||
|
||||
float wheel_base;
|
||||
float max_turn_angle;
|
||||
|
||||
} _parameters{}; /**< local copies of interesting parameters */
|
||||
|
||||
struct {
|
||||
param_t l1_period;
|
||||
param_t l1_damping;
|
||||
param_t l1_distance;
|
||||
|
||||
param_t gndspeed_trim;
|
||||
param_t gndspeed_max;
|
||||
|
||||
param_t speed_control_mode;
|
||||
|
||||
param_t speed_p;
|
||||
param_t speed_i;
|
||||
param_t speed_d;
|
||||
param_t speed_imax;
|
||||
param_t throttle_speed_scaler;
|
||||
|
||||
param_t throttle_min;
|
||||
param_t throttle_max;
|
||||
param_t throttle_cruise;
|
||||
param_t throttle_slew_max;
|
||||
|
||||
param_t wheel_base;
|
||||
param_t max_turn_angle;
|
||||
} _parameter_handles{}; /**< handles for interesting parameters */
|
||||
|
||||
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
*/
|
||||
int parameters_update();
|
||||
|
||||
void manual_control_setpoint_poll();
|
||||
void position_setpoint_triplet_poll();
|
||||
void vehicle_control_mode_poll();
|
||||
void vehicle_attitude_poll();
|
||||
|
||||
/**
|
||||
* Control position.
|
||||
*/
|
||||
bool control_position(const matrix::Vector2f &global_pos, const matrix::Vector3f &ground_speed,
|
||||
const position_setpoint_triplet_s &_pos_sp_triplet);
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
*/
|
||||
static int task_main_trampoline(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Main sensor collection task.
|
||||
*/
|
||||
void task_main();
|
||||
|
||||
};
|
||||
@@ -0,0 +1,288 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2017 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 rover_pos_control_params.c
|
||||
*
|
||||
* Parameters defined by the position control task for ground rovers
|
||||
*
|
||||
* This is a modification of the fixed wing params and it is designed for ground rovers.
|
||||
* It has been developed starting from the fw module, simplified and improved with dedicated items.
|
||||
*
|
||||
* All the ackowledgments and credits for the fw wing app are reported in those files.
|
||||
*
|
||||
* @author Marco Zorzi <mzorzi@student.ethz.ch>
|
||||
*/
|
||||
|
||||
/*
|
||||
* Controller parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
/**
|
||||
* L1 distance
|
||||
*
|
||||
* This is the waypoint radius
|
||||
*
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @max 100.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_L1_DIST, 5.0f);
|
||||
|
||||
/**
|
||||
* L1 period
|
||||
*
|
||||
* This is the L1 distance and defines the tracking
|
||||
* point ahead of the rover it's following.
|
||||
* Using values around 2-5 for a traxxas stampede. Shorten
|
||||
* slowly during tuning until response is sharp without oscillation.
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @max 50.0
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 10.0f);
|
||||
|
||||
/**
|
||||
* L1 damping
|
||||
*
|
||||
* Damping factor for L1 control.
|
||||
*
|
||||
* @min 0.6
|
||||
* @max 0.9
|
||||
* @decimal 2
|
||||
* @increment 0.05
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_L1_DAMPING, 0.75f);
|
||||
|
||||
/**
|
||||
* Cruise throttle
|
||||
*
|
||||
* This is the throttle setting required to achieve the desired cruise speed.
|
||||
* 10% is ok for a traxxas stampede vxl with ESC set to training mode
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_THR_CRUISE, 0.1f);
|
||||
|
||||
/**
|
||||
* Throttle limit max
|
||||
*
|
||||
* This is the maximum throttle % that can be used by the controller.
|
||||
* For a Traxxas stampede vxl with the ESC set to training, 30 % is enough
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f);
|
||||
|
||||
/**
|
||||
* Throttle limit min
|
||||
*
|
||||
* This is the minimum throttle % that can be used by the controller.
|
||||
* Set to 0 for rover
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f);
|
||||
|
||||
/**
|
||||
* Idle throttle
|
||||
*
|
||||
* This is the minimum throttle while on the ground, it should be 0 for a rover
|
||||
*
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.0
|
||||
* @max 0.4
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_THR_IDLE, 0.0f);
|
||||
|
||||
/**
|
||||
* Control mode for speed
|
||||
*
|
||||
* This allows the user to choose between closed loop gps speed or open loop cruise throttle speed
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @value 0 open loop control
|
||||
* @value 1 close the loop with gps speed
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1);
|
||||
|
||||
/**
|
||||
* Speed proportional gain
|
||||
*
|
||||
* This is the proportional gain for the speed closed loop controller
|
||||
*
|
||||
* @unit %m/s
|
||||
* @min 0.005
|
||||
* @max 50.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_SPEED_P, 2.0f);
|
||||
|
||||
/**
|
||||
* Speed Integral gain
|
||||
*
|
||||
* This is the integral gain for the speed closed loop controller
|
||||
*
|
||||
* @unit %m/s
|
||||
* @min 0.00
|
||||
* @max 50.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_SPEED_I, 0.1f);
|
||||
|
||||
/**
|
||||
* Speed proportional gain
|
||||
*
|
||||
* This is the derivative gain for the speed closed loop controller
|
||||
*
|
||||
* @unit %m/s
|
||||
* @min 0.00
|
||||
* @max 50.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_SPEED_D, 0.0f);
|
||||
|
||||
/**
|
||||
* Speed integral maximum value
|
||||
*
|
||||
* This is the maxim value the integral can reach to prevent wind-up.
|
||||
*
|
||||
* @unit %m/s
|
||||
* @min 0.005
|
||||
* @max 50.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Speed to throttle scaler
|
||||
*
|
||||
* This is a gain to map the speed control output to the throttle linearly.
|
||||
*
|
||||
* @unit %m/s
|
||||
* @min 0.005
|
||||
* @max 50.0
|
||||
* @decimal 3
|
||||
* @increment 0.005
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f);
|
||||
|
||||
/**
|
||||
* Trim ground speed
|
||||
*
|
||||
*
|
||||
* @unit m/s
|
||||
* @min 0.0
|
||||
* @max 40
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
|
||||
|
||||
/**
|
||||
* Maximum ground speed
|
||||
*
|
||||
*
|
||||
* @unit m/s
|
||||
* @min 0.0
|
||||
* @max 40
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
|
||||
|
||||
/**
|
||||
* Distance from front axle to rear axle
|
||||
*
|
||||
*
|
||||
* @unit m
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 2.0f);
|
||||
|
||||
/**
|
||||
* Maximum turn angle for Ackerman steering.
|
||||
* At a control output of 0, the steering wheels are at 0 radians.
|
||||
* At a control output of 1, the steering wheels are at GND_MAX_ANG radians.
|
||||
*
|
||||
* @unit rad
|
||||
* @min 0.0
|
||||
* @max 3.14159
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Rover Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
|
||||
Reference in New Issue
Block a user