Hello all
I’m very new to using PX4 and having a problem in implementing a custom position controller. for quadrotors.
I’m currently using MAVROS to publish setpoints for position and velocity (offboard control mode)
I tried implementing Sliding Mode Control and LQR Control method to PositionControl.cpp
It worked just fine when using QGroundControl and uploading waypoint missions. But when I transition to offboard control the controller gets messed up and LQR control even drops the quadrotor.
I found that the problem is the position setpoint and velocity setpoint isn’t set at the same time as seen in the picture below.
When the position controller gets the published setpoints, either Position or the Velocity setpoint is always set to NAN which makes the thrust and attitude setpoint to NAN as well.
So the question is…
Is there a way to make the position controller to subscribe both position and velocity setpoints at the same time?
Or maybe calculate the derivative of position setpoints internally in the position controller? → in this case how do i get the previous setpoint?
here are my codes
offbd_node.cpp
/**
* @file offbd_node.cpp
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
* Stack and tested in Gazebo SITL
*/
#include <ros/ros.h>
#include <math.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
double r=1.0;
double theta;
double count=0.0;
double wn=1.0;
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "offbd_node");
ros::NodeHandle nh;
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
ros::Publisher cmd_vel_pub = nh.advertise<geometry_msgs::TwistStamped>
("mavros/setpoint_velocity/cmd_vel", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(100.0);
// wait for FCU connection
while(ros::ok() && !current_state.connected){
ros::spinOnce();
rate.sleep();
}
geometry_msgs::PoseStamped pose;
geometry_msgs::TwistStamped vel;
pose.header.stamp = ros::Time::now();
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 10;
vel.header.stamp = ros::Time::now();
vel.twist.linear.x = 0;
vel.twist.linear.y = 0;
vel.twist.linear.z = 0;
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
mavros_msgs::SetMode offbd_set_mode;
offbd_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
while(ros::ok()){
if( current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offbd_set_mode) &&
offbd_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
} else {
if( !current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
}
theta = wn*count*0.005;
pose.header.stamp = ros::Time::now();
pose.pose.position.x = r*sin(theta);
pose.pose.position.y = r*cos(theta);
pose.pose.position.z = 10;
vel.header.stamp = ros::Time::now();
vel.twist.linear.x = wn*r*cos(theta);
vel.twist.linear.y = -wn*r*sin(theta);
vel.twist.linear.z = 0;
count++;
local_pos_pub.publish(pose);
cmd_vel_pub.publish(vel);
ros::spinOnce();
rate.sleep();
}
return 0;
}
positioncontrol.cpp
/****************************************************************************
*
* Copyright (c) 2018 - 2019 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 PositionControl.cpp
*/
#include "PositionControl.hpp"
#include "ControlMath.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/defines.h>
#include <geo/geo.h>
#include <iostream>
#define TSSMC false
#define SMC false
#define LQR true
using namespace std;
using namespace matrix;
void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
{
_gain_vel_p = P;
_gain_vel_i = I;
_gain_vel_d = D;
}
void PositionControl::setVelocityLimits(const float vel_horizontal, const float vel_up, const float vel_down)
{
_lim_vel_horizontal = vel_horizontal;
_lim_vel_up = vel_up;
_lim_vel_down = vel_down;
}
void PositionControl::setThrustLimits(const float min, const float max)
{
// make sure there's always enough thrust vector length to infer the attitude
_lim_thr_min = math::max(min, 10e-4f);
_lim_thr_max = max;
}
void PositionControl::setHorizontalThrustMargin(const float margin)
{
_lim_thr_xy_margin = margin;
}
void PositionControl::updateHoverThrust(const float hover_thrust_new)
{
// Given that the equation for thrust is T = a_sp * Th / g - Th
// with a_sp = desired acceleration, Th = hover thrust and g = gravity constant,
// we want to find the acceleration that needs to be added to the integrator in order obtain
// the same thrust after replacing the current hover thrust by the new one.
// T' = T => a_sp' * Th' / g - Th' = a_sp * Th / g - Th
// so a_sp' = (a_sp - g) * Th / Th' + g
// we can then add a_sp' - a_sp to the current integrator to absorb the effect of changing Th by Th'
if (hover_thrust_new > FLT_EPSILON) {
_vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * _hover_thrust / hover_thrust_new + CONSTANTS_ONE_G - _acc_sp(2);
setHoverThrust(hover_thrust_new);
}
}
void PositionControl::setState(const PositionControlStates &states)
{
_pos = states.position;
_vel = states.velocity;
_yaw = states.yaw;
_vel_dot = states.acceleration;
}
void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
{
_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
_acc_sp = Vector3f(setpoint.acceleration);
_yaw_sp = setpoint.yaw;
_yawspeed_sp = setpoint.yawspeed;
}
bool PositionControl::update(const float dt)
{
bool valid = _inputValid();
if (valid) {
_positionControl(dt);
_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
}
// There has to be a valid output acceleration and thrust setpoint otherwise something went wrong
valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2));
valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));
return valid;
}
void PositionControl::_positionControl(const float dt)
{
#if TSSMC
//altitude control
Vector2f Kz1 = Vector2f(16.1,5.07);
Vector2f Kz2 = Vector2f(29.79,9,38);
Vector2f Gz = Vector2f(0,-0.65);
// error state
Vector2f Xz = Vector2f(_pos(2) - _pos_sp(2),_vel(2) - _vel_sp(2));
float _dataAz[4] = {0, 1, 0, 0};
//TSfuzzy
Matrix<float, 2, 2> Az(_dataAz);
Vector2f Bz1 = Vector2f(0,-1.54);
Vector2f Bz2 = Vector2f(0,-0.77);
ksi1_max = 1.54;
ksi1_min = 0.77;
ksi = m/(cos())
hz1 =
hz2 =
Matrix<float, 2, 2> Az_bar = Az + hz1*hz1*(Bz1*Kz1)
Sz = Xz(0)*Gz(0) + Xz(1)*Gz(1);
Sz_sign = sign(Sz);
Uz0 = hz1*Kz1*Xz + hz2*Kz2*Xz;
#endif
#if SMC
Vector3f k1 = Vector3f(2*3.35, 2*3.35, 4*1.3);
Vector3f k2 = Vector3f(1*0.05, 1*0.05, 1*0.3);
Vector3f e = _pos - _pos_sp;
Vector3f e_dot = _vel - _vel_sp;
Vector3f S = k1.emult(e) + e_dot;
Vector3f SW = Vector3f (sign(S(0))*k2(0),sign(S(1))*k2(1), sign(S(2))*k2(2));
Vector3f acc_sp_velocity = e_dot.emult(k1) + SW;
ControlMath::addIfNotNanVector3f(_acc_sp, -acc_sp_velocity);
_accelerationControl();
#endif
#if LQR
float m = 1.51;
// state feedback gain
float _dataKz[2] = {4.0825, 3.7411};
Matrix<float, 1, 2> Kz(_dataKz);
float _dataKp[8] = {7.0711, 0, 4.9687, 0, 0, 7.0711, 0, 4.9687};
Matrix<float, 2, 4> Kp(_dataKp);
Vector3f e = _pos - _pos_sp;
Vector3f e_dot = _vel - _vel_sp;
//altitude control
Vector2f ez = Vector2f(e(2), e_dot(2));
Vector <float, 1> Uz = -Kz*ez;
//position control
float _dataep[4] = {e(0), e(1), e_dot(0), e_dot(1)};
Vector<float, 4> ep(_dataep);
cout << '\n' << _pos_sp(0) << ',' << _pos_sp(1) << ',' << _vel_sp(0) << ',' << _vel_sp(1) << '\n';
Vector2f Up = -Kp*ep;
Vector3f U = Vector3f(Up(0)/m, Up(1)/m, Uz(0)/m);
// All nan if not takeoff or have a mission
ControlMath::addIfNotNanVector3f(_acc_sp, U);
//cout << _acc_sp(0) << ',' << _acc_sp(1) << ',' << _acc_sp(2) << '\n';
_accelerationControl();
#endif
// Saturate maximal vertical thrust
_thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max);
// Get allowed horizontal thrust after prioritizing vertical control
const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
const float thrust_max_xy_squared = thrust_max_squared - thrust_z_squared;
float thrust_max_xy = 0;
if (thrust_max_xy_squared > 0) {
thrust_max_xy = sqrtf(thrust_max_xy_squared);
}
// Saturate thrust in horizontal direction
const Vector2f thrust_sp_xy(_thr_sp);
const float thrust_sp_xy_norm = thrust_sp_xy.norm();
if (thrust_sp_xy_norm > thrust_max_xy) {
_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
}
}
void PositionControl::_accelerationControl()
{
// Assume standard acceleration due to gravity in vertical direction for attitude generation
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
// Scale thrust assuming hover thrust produces standard gravity
float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
// Project thrust to planned body attitude
collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));
collective_thrust = math::min(collective_thrust, -_lim_thr_min);
_thr_sp = body_z * collective_thrust;
}
bool PositionControl::_inputValid()
{
bool valid = true;
// Every axis x, y, z needs to have some setpoint
for (int i = 0; i <= 2; i++) {
valid = valid && (PX4_ISFINITE(_pos_sp(i)) || PX4_ISFINITE(_vel_sp(i)) || PX4_ISFINITE(_acc_sp(i)));
}
// x and y input setpoints always have to come in pairs
valid = valid && (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1)));
valid = valid && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1)));
valid = valid && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1)));
// For each controlled state the estimate has to be valid
for (int i = 0; i <= 2; i++) {
if (PX4_ISFINITE(_pos_sp(i))) {
valid = valid && PX4_ISFINITE(_pos(i));
}
if (PX4_ISFINITE(_vel_sp(i))) {
valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i));
}
}
return valid;
}
void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const
{
local_position_setpoint.x = _pos_sp(0);
local_position_setpoint.y = _pos_sp(1);
local_position_setpoint.z = _pos_sp(2);
local_position_setpoint.yaw = _yaw_sp;
local_position_setpoint.yawspeed = _yawspeed_sp;
local_position_setpoint.vx = _vel_sp(0);
local_position_setpoint.vy = _vel_sp(1);
local_position_setpoint.vz = _vel_sp(2);
_acc_sp.copyTo(local_position_setpoint.acceleration);
_thr_sp.copyTo(local_position_setpoint.thrust);
}
void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
{
ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
}