【PX4飞控】二次开发1—加速度转期望姿态算法修改

版本说明:

PX4 v1.14.0 的加速度转换姿态四元数代码有些问题,z轴期望加速度不为0时,xy轴期望加速度会移入额外误差,这个 BUG 在 PX4 v1.15.1 中已经修正,但是又有额外的问题。因此还是使用 v1.14.0,只是修复相关代码。

先比较一下 1.14.0 与 1.15.1 源码

复制代码
https://github.com/PX4/PX4-Autopilot/compare/1e4fcfc..2e6dd24

搜索 src/modules/mc_pos_control/PositionControl/PositionControl.cpp

查看两个版本 void PositionControl::_accelerationControl() 函数的代码对比

v1.14.0

cpp 复制代码
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;
}

v1.15.1

cpp 复制代码
void PositionControl::_accelerationControl()
{
	// Assume standard acceleration due to gravity in vertical direction for attitude generation
	float z_specific_force = -CONSTANTS_ONE_G;

	if (!_decouple_horizontal_and_vertical_acceleration) {
		// Include vertical acceleration setpoint for better horizontal acceleration tracking
		z_specific_force += _acc_sp(2);
	}

	Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized();
	ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
	// Convert to thrust assuming hover thrust produ	// Modified by Zhang San, based on v1.15.1.ces standard gravity
	const float thrust_ned_z = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
	// Project thrust to planned body attitude
	const float cos_ned_body = (Vector3f(0, 0, 1).dot(body_z));
	const float collective_thrust = math::min(thrust_ned_z / cos_ned_body, -_lim_thr_min);
	_thr_sp = body_z * collective_thrust;
}

注意到_decouple_horizontal_and_vertical_acceleration,在PositionControl.hpp中定义。

cpp 复制代码
bool _decouple_horizontal_and_vertical_acceleration{false}; ///< Ignore vertical acceleration setpoint to remove its effect on the tilt setpoint
void decoupleHorizontalAndVecticalAcceleration(bool val) { _decouple_horizontal_and_vertical_acceleration = val; }

MulticopterPositionControl.cpp 中调用了decoupleHorizontalAndVecticalAcceleration() 函数将该值设置为MPC_ACC_DECOUPLE的值。

cpp 复制代码
_control.decoupleHorizontalAndVecticalAcceleration(_param_mpc_acc_decouple.get());

可以看到 1.15.1 新引入了一个参数 MPC_ACC_DECOUPLE。来控制 Z轴的加速度控制策略。

  • MPC_ACC_DECOUPLE (INT32)

    Acceleration to tilt coupling.

    Set to decouple tilt from vertical acceleration. This provides smoother flight but slightly worse tracking in position and auto modes. Unset if accurate position tracking during dynamic maneuvers is more important than a smooth flight.

    Reboot minValue maxValue increment default unit
    Enabled (1)

    参考

    Parameter Reference | PX4 Guide (main)

该参数默认值为 Enable(解耦),所以存在和 PX4 1.14.0 一样的问题。需要手动设置为 false。这里我们修改 1.14.0 的源码,不用管该参数。直接将函数改为

cpp 复制代码
// Modified by Zhang San, based on v1.15.1.
void PositionControl::_accelerationControl()
{
	// Assume standard acceleration due to gravity in vertical direction for attitude generation
	float z_specific_force = -CONSTANTS_ONE_G;

	// if (!_decouple_horizontal_and_vertical_acceleration) {
		// Include vertical acceleration setpoint for better horizontal acceleration tracking
		z_specific_force += _acc_sp(2);
	// }

	Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized();
	ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
	// Convert to thrust assuming hover thrust produces standard gravity
	const float thrust_ned_z = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
	// Project thrust to planned body attitude
	const float cos_ned_body = (Vector3f(0, 0, 1).dot(body_z));
	const float collective_thrust = math::min(thrust_ned_z / cos_ned_body, -_lim_thr_min);
	_thr_sp = body_z * collective_thrust;
}

然后编译飞控即可

相关推荐
屁股割了还要学31 分钟前
【C语言进阶】柔性数组
c语言·开发语言·数据结构·c++·学习·算法·柔性数组
oioihoii1 小时前
C++实战案例:从static成员到线程安全的单例模式
java·c++·单例模式
☞下凡☜1 小时前
C语言(20250722)
linux·c语言·开发语言
您的通讯录好友1 小时前
TechGPT2部署
linux·人工智能·python·大模型·techgpt
charlie1145141912 小时前
快速入门Socket编程——封装一套便捷的Socket编程——导论
linux·网络·笔记·面试·网络编程·socket
zzzzz_ccc2 小时前
AVL树和红黑树的特性以及模拟实现
c语言·数据结构·c++
WSSWWWSSW2 小时前
认识自我的机器人:麻省理工学院基于视觉的系统让机器了解自身机体
人工智能·机器人
liulilittle3 小时前
C++ Proactor 与 Reactor 网络编程模式
开发语言·网络·c++·reactor·proactor
贺贺丿3 小时前
Docker4-容器化企业级应用
linux·nginx·docker·云原生·eureka·tomcat·ssh
唐青枫3 小时前
Linux 如何统计系统上各个用户登录(或者登出)记录出现的次数?
linux