ardupilot 增加新的定点控制实现刹车
时间：2022-12-02 12:30:00
目录
文章目录
- 目录
- 摘要
- 1.新增position模式
- 2.实现position模式初始化
- 3.实现position模式运行函数
- 4.实现position模式关键函数
-
- 1.get_pilot_desired_value(target_roll, target_pitch)
- 2.position_nav->set_pilot_desired_speed(target_roll, target_pitch, G_Dt);
- 3.position_nav->pos_update();
-
- 1. mode_position_calc_desired_velocity(dt); //计算目标速度
- 2. _pos_control.update_position_xy_controller()//位置控制器更新
- 5.附带函数和定义
-
- 1.AC_Loiter.cpp
摘要
本节主要记录增加ardupilot位置控制模式的代码，apm loiter制动效果不理想。新增position 控制模式，实现原理：apm遥杆输入控制量转换成期望速度，期望速度积分得到目标位置，然后进行位置PID位置速度控制PID控制，最后将速度环控输出转换为身体坐标系，进行姿态控制！在测试这种模式之前，需要注意的是：1。设置三种飞机模式：1。 2.loiter模式 3.新增的poistion这样做的好处是，当发生事故时，可以立即切换到loiter模式，防止造成太大损伤，2.增加飞行日志，实时记录飞机的各种速度，加速度，角度变化信息，方便后续分析。
1.新增position模式
调用更新实现模式
Mode *Copter::mode_from_mode_num(const Mode::Number mode) {
Mode *ret = nullptr; switch (mode) {
#if MODE_ACRO_ENABLED == ENABLED case Mode::Number::ACRO: ///速率模式 ret = &mode_acro; break; #endif case Mode::Number::STABILIZE: ret = &mode_stabilize;//自稳模式 break; case Mode::Number::ALT_HOLD: ret = &mode_althold;///定高模式 break; #if MODE_AUTO_ENABLED == ENABLED case Mode::Number::AUTO: ret = &mode_auto;
//自动模式
break
;
#endif
#if MODE_CIRCLE_ENABLED == ENABLED
case Mode
::Number
::CIRCLE
: ret
=
&mode_circle
;
break
;
#endif
#if MODE_LOITER_ENABLED == ENABLED
case Mode
::Number
::LOITER
:
//定点模式 ret
=
&mode_loiter
;
break
;
#endif
#if MODE_POSITION_ENABLED == ENABLED
case Mode
::Number
::POSITION
:
//定点模式 ret
=
&mode_position
;
break
;
#endif
#if MODE_GUIDED_ENABLED == ENABLED
case Mode
::Number
::GUIDED
: ret
=
&mode_guided
;
break
;
#endif
case Mode
::Number
::LAND
: ret
=
&mode_land
;
break
;
#if MODE_RTL_ENABLED == ENABLED
case Mode
::Number
::RTL
: ret
=
&mode_rtl
;
break
;
#endif
#if MODE_DRIFT_ENABLED == ENABLED
case Mode
::Number
::DRIFT
: ret
=
&mode_drift
;
break
;
#endif
#if MODE_SPORT_ENABLED == ENABLED
case Mode
::Number
::SPORT
: ret
=
&mode_sport
;
break
;
#endif
#if MODE_FLIP_ENABLED == ENABLED
case Mode
::Number
::FLIP
: ret
=
&mode_flip
;
break
;
#endif
#if AUTOTUNE_ENABLED == ENABLED
case Mode
::Number
::AUTOTUNE
: ret
=
&mode_autotune
;
break
;
#endif
#if MODE_POSHOLD_ENABLED == ENABLED
case Mode
::Number
::POSHOLD
: ret
=
&mode_poshold
;
break
;
#endif
#if MODE_BRAKE_ENABLED == ENABLED
case Mode
::Number
::BRAKE
: ret
=
&mode_brake
;
break
;
#endif
#if MODE_THROW_ENABLED == ENABLED
case Mode
::Number
::THROW
: ret
=
&mode_throw
;
break
;
#endif
#if ADSB_ENABLED == ENABLED
case Mode
::Number
::AVOID_ADSB
: ret
=
&mode_avoid_adsb
;
break
;
#endif
#if MODE_GUIDED_NOGPS_ENABLED == ENABLED
case Mode
::Number
::GUIDED_NOGPS
: ret
=
&mode_guided_nogps
;
break
;
#endif
#if MODE_SMARTRTL_ENABLED == ENABLED
case Mode
::Number
::SMART_RTL
: ret
=
&mode_smartrtl
;
break
;
#endif
#if OPTFLOW == ENABLED
case Mode
::Number
::FLOWHOLD
: ret
=
(Mode
*
)g2
.mode_flowhold_ptr
;
break
;
#endif
#if MODE_FOLLOW_ENABLED == ENABLED
case Mode
::Number
::FOLLOW
: ret
=
&mode_follow
;
break
;
#endif
#if MODE_ZIGZAG_ENABLED == ENABLED
case Mode
::Number
::ZIGZAG
: ret
=
&mode_zigzag
;
break
;
#endif
#if MODE_SYSTEMID_ENABLED == ENABLED
case Mode
::Number
::SYSTEMID
: ret
=
(Mode
*
)g2
.mode_systemid_ptr
;
break
;
#endif
#if MODE_AUTOROTATE_ENABLED == ENABLED
case Mode
::Number
::AUTOROTATE
: ret
=
&mode_autorotate
;
break
;
#endif
default
:
break
;
}
return ret
;
}
定义position类及对象
#if MODE_POSITION_ENABLED == ENABLED
ModePosition mode_position;
#endif
//位置控制模式代码,新增代码
class ModePosition : public Mode {
public:
//继承构造函数
using Mode::Mode;
bool init(bool ignore_checks) override; //模式初始化函数
void run() override;
bool requires_GPS() const override {
return true; }
bool has_manual_throttle() const override {
return false; }
bool allows_arming(bool from_gcs) const override {
return true; };
bool is_autopilot() const override {
return false; }
bool has_user_takeoff(bool must_navigate) const override {
return true; }
#if PRECISION_LANDING == ENABLED
void set_precision_position_enabled(bool value)
{
_precision_position_enabled = value; //进行精准悬停使能
}
#endif
protected:
const char *name() const override {
return "POSITION"; }
const char *name4() const override {
return "POSI"; }
uint32_t wp_distance() const override;
int32_t wp_bearing() const override;
#if PRECISION_LANDING == ENABLED
bool do_precision_position();
void precision_position_xy();
#endif
private:
#if PRECISION_LANDING == ENABLED
bool _precision_position_enabled;
#endif
};
到这里记得实现对象初始化
Mode::Mode(void) :
g(copter.g),
g2(copter.g2),
wp_nav(copter.wp_nav),
loiter_nav(copter.loiter_nav),
position_nav(copter.position_nav),//导航信息定义
pos_control(copter.pos_control),
inertial_nav(copter.inertial_nav),
ahrs(copter.ahrs),
attitude_control(copter.attitude_control),
motors(copter.motors),
channel_roll(copter.channel_roll),
channel_pitch(copter.channel_pitch),
channel_throttle(copter.channel_throttle),
channel_yaw(copter.channel_yaw),
G_Dt(copter.G_Dt)
{
};
到system.cpp的 void Copter::allocate_motors(void)函数实现初始化
//位置导航对象
position_nav = new AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control);
if (position_nav == nullptr)
{
AP_HAL::panic("Unable to allocate LoiterNav");
}
AP_Param::load_object_from_eeprom(position_nav, position_nav->var_info);
注意新增mode_position.cpp文件
2.实现position模式初始化
bool ModePosition::init(bool ignore_checks)
{
//判断遥控器没有故障,执行下面函数
if (!copter.failsafe.radio)
{
//定义目标横滚角、目标俯仰角度
int16_t target_roll, target_pitch;
//对飞行员输入申请简单模式转换
update_simple_mode(); //这里看是设置哪种类型
//获取飞行员目前输入值,限制最大飞行速度新
get_pilot_desired_value(target_roll, target_pitch);
//处理飞行员的横滚和俯仰值到速度
position_nav->set_pilot_desired_speed(target_roll, target_pitch, G_Dt);
//设定默认刹车最大角度信息
position_nav->set_max_brake_angle(position_nav->_brake_angle);
} else
{
//清除飞行员期望的加速度,以防无线电故障保护事件发生,并且由于某种原因我们没有切换到RTL
position_nav->clear_pilot_desired_speed();
}
//初始化悬停目标
position_nav->init_target();
//初始化位置和目标速度信息------- initialise position and desired velocity
if (!pos_control->is_active_z())
{
pos_control->set_alt_target_to_current_alt();
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
}
return true;
}
3.实现position模式运行函数
//定义最大刹车角度 extern int16_t ym_uav_position_control_brake_angle; void ModePosition::run() { int16_t target_roll, target_pitch; float target_yaw_rate = 0.0f; float target_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f; //初始化垂直速度和加速度----- initialize vertical speed and acceleration pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_max_accel_z(g.pilot_accel_z); //只要是遥控器没有故障，就处理飞行员输入---- process pilot inputs unless we are in radio failsafe if (!copter.failsafe.radio) { //将飞行员输入转换成简单模式------ apply SIMPLE mode transform to pilot inputs update_simple_mode(); //获取飞行员目前输入值,限制最大飞行速度新 get_pilot_desired_value(target_roll, target_pitch); //增加日志记录信息 ym_log_position_target_roll=target_roll; ym_log_position_target_pitch=target_pitch; //处理飞行员的横滚和俯仰值到速度 position_nav->set_pilot_desired_speed(target_roll, target_pitch, G_Dt); //设定刹车角度信息 ym_uav_position_control_brake_angle=position_nav->get_max_brake_angle(); //处理飞行员偏航输入------ get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); //获得飞行员想要的爬升速度------ get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); //对垂直速度进行限制 target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); } else { // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason //清除飞行员期望的加速度，以防无线电故障保护事件发生，并且由于某种原因我们没有切换到RTL position_nav->clear_pilot_desired_acceleration(); } //如果我们可能着陆了,释放悬停目标 relax loiter target if we might be landed if (copter.ap.land_complete_maybe) { position_nav->soften_for_landing(); } //悬停模式状态机识别------Loiter State Machine Determination AltHoldModeState loiter_state = get_alt_hold_state(target_climb_rate); //悬停模式状态机------Loiter State Machine switch (loiter_state) { case AltHold_MotorStopped: //电机停转 //复位速度控制积分项 attitude_control->reset_rate_controller_I_terms(); //设定当前机头为目标机头 attitude_control->set_yaw_target_to_current_heading(); //释放高度控制,强制油门输出为0 pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero //初始化位置控制器 position_nav->init_target(); //进行横滚、俯仰、偏航姿态控制 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(position_nav->get_roll(), position_nav->get_pitch(), target_yaw_rate); //进行高度控制 pos_control->update_z_controller(); break; case AltHold_Takeoff: //电机起飞 //初始化起飞-----initiate take-off if (!takeoff.running()) { //默认起飞高度是0,最大不超过10m takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); } //获取起飞调节飞行器和起飞爬升速度------ get takeoff adjusted pilot and takeoff climb rates takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate); //获取避障调节攀升速度---- get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); //运行悬停控制器------- run loiter controller position_nav->pos_update(); //调用姿态控制器------call attitude controller //悬停位置控制器：输入量（目标横滚角,目标俯仰角度,目标偏航角速度） attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(position_nav->get_roll(), position_nav->get_pitch(), target_yaw_rate); //更新高度目标和调用位置控制器------update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); //获取目标高度 pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); //进行位置控制操作 pos_control->update_z_controller(); break; case AltHold_Landed_Ground_Idle: attitude_control->set_yaw_target_to_current_heading(); // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms(); position_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); break; case AltHold_Flying: //设置电机到全速运动范围------- set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); #if PRECISION_LANDING == ENABLED if (do_precision_position()) { precision_position_xy(); } #endif //运行位置控制器------ run loiter controller position_nav->pos_update(); //调用姿态控制器------ call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(position_nav->get_roll(), position_nav->get_pitch(), target_yaw_rate)元器件数据手册、IC替代型号，打造电子元器件IC百科大全！
