目录
摘要
本节主要记录自己学ardupilot的避障代码过程,首先看下怎么注册避障传感器,然后怎么获取避障数据,最后怎么实现避障控制。
1.注册避障模块实现初始化
**1》》》》》》》》》》**
void Copter::setup()
{
cliSerial = hal.console;
//加载初始化参数表到 in var_info[]s
AP_Param::setup_sketch_defaults();
// 初始化无人机的结构布局
StorageManager::set_layout_copter();
//注册传感器
init_ardupilot();
// 初始化 main loop 任务
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
// 初始化 main loop 任务
perf_info_reset();
fast_loopTimer = AP_HAL::micros();
}
**2》》》》》》》》》》**
void Copter::init_ardupilot()
{
init_proximity();//避障函数初始化
}
**3》》》》》》》》》》**
void Copter::init_proximity(void)
{
#if PROXIMITY_ENABLED == ENABLED
g2.proximity.init();
g2.proximity.set_rangefinder(&rangefinder);
#endif
}
------------------------------------------------------------
首先看下 g2.proximity.init();
------------------------------------------------------------
void AP_Proximity::init(void)
{
if (num_instances != 0) //没有传感器就直接返回
{
//初始化号召等待2s-------- init called a 2nd time?
return;
}
for (uint8_t i=0; i<PROXIMITY_MAX_INSTANCES; i++)
{
detect_instance(i); //传感器识别
if (drivers[i] != nullptr) //我们为这个实例加载了一个驱动程序,所以它必须存在(尽管它可能不健康)
{
// we loaded a driver for this instance, so it must be present (although it may not be healthy)
num_instances = i+1;
}
//初始化状态标志-----initialise status
state[i].status = Proximity_NotConnected;
}
}
------------------------------------------------------------
继续看下识别我们用的哪种传感器 detect_instance(i);
------------------------------------------------------------
void AP_Proximity::detect_instance(uint8_t instance)
{
uint8_t type = _type[instance];
if (type == Proximity_Type_SF40C) //激光测距模块
{
if (AP_Proximity_LightWareSF40C::detect(serial_manager))
{
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], serial_manager);
return;
}
}
if (type == Proximity_Type_MAV)
{
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_MAV(*this, state[instance]);
return;
}
if (type == Proximity_Type_TRTOWER)
{
if (AP_Proximity_TeraRangerTower::detect(serial_manager))
{
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], serial_manager);
return;
}
}
if (type == Proximity_Type_RangeFinder) //测距仪器
{
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]);
return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (type == Proximity_Type_SITL)
{
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_SITL(*this, state[instance]); //创建实例
return;
}
#endif
}
///////////////////////////////////////////////
可以看到这里主要是选择哪种类型的避障模块
///////////////////////////////////////////////
1》这里看下支持的驱动类型:
//2》假如我们使用的是Proximity_Type_RangeFinder,则要创建对象
drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]);
3》用到的参数
// table of user settable parameters
const AP_Param::GroupInfo AP_Proximity::var_info[] = {
// 0 is reserved for possible addition of an ENABLED parameter
// @Param: _TYPE
// @DisplayName: Proximity type
// @Description: What type of proximity sensor is connected
// @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder
// @User: Standard
AP_GROUPINFO("_TYPE", 1, AP_Proximity, _type[0], 0),
// @Param: _ORIENT
// @DisplayName: Proximity sensor orientation
// @Description: Proximity sensor orientation
// @Values: 0:Default,1:Upside Down
// @User: Standard
AP_GROUPINFO("_ORIENT", 2, AP_Proximity, _orientation[0], 0),
// @Param: _YAW_CORR
// @DisplayName: Proximity sensor yaw correction
// @Description: Proximity sensor yaw correction
// @Units: degrees
// @Range: -180 180
// @User: Standard
AP_GROUPINFO("_YAW_CORR", 3, AP_Proximity, _yaw_correction[0], PROXIMITY_YAW_CORRECTION_DEFAULT),
// @Param: _IGN_ANG1
// @DisplayName: Proximity sensor ignore angle 1
// @Description: Proximity sensor ignore angle 1
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG1", 4, AP_Proximity, _ignore_angle_deg[0], 0),
// @Param: _IGN_WID1
// @DisplayName: Proximity sensor ignore width 1
// @Description: Proximity sensor ignore width 1
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID1", 5, AP_Proximity, _ignore_width_deg[0], 0),
// @Param: _IGN_ANG2
// @DisplayName: Proximity sensor ignore angle 2
// @Description: Proximity sensor ignore angle 2
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG2", 6, AP_Proximity, _ignore_angle_deg[1], 0),
// @Param: _IGN_WID2
// @DisplayName: Proximity sensor ignore width 2
// @Description: Proximity sensor ignore width 2
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID2", 7, AP_Proximity, _ignore_width_deg[1], 0),
// @Param: _IGN_ANG3
// @DisplayName: Proximity sensor ignore angle 3
// @Description: Proximity sensor ignore angle 3
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG3", 8, AP_Proximity, _ignore_angle_deg[2], 0),
// @Param: _IGN_WID3
// @DisplayName: Proximity sensor ignore width 3
// @Description: Proximity sensor ignore width 3
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID3", 9, AP_Proximity, _ignore_width_deg[2], 0),
// @Param: _IGN_ANG4
// @DisplayName: Proximity sensor ignore angle 4
// @Description: Proximity sensor ignore angle 4
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG4", 10, AP_Proximity, _ignore_angle_deg[3], 0),
// @Param: _IGN_WID4
// @DisplayName: Proximity sensor ignore width 4
// @Description: Proximity sensor ignore width 4
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID4", 11, AP_Proximity, _ignore_width_deg[3], 0),
// @Param: _IGN_ANG5
// @DisplayName: Proximity sensor ignore angle 5
// @Description: Proximity sensor ignore angle 5
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG5", 12, AP_Proximity, _ignore_angle_deg[4], 0),
// @Param: _IGN_WID5
// @DisplayName: Proximity sensor ignore width 5
// @Description: Proximity sensor ignore width 5
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID5", 13, AP_Proximity, _ignore_width_deg[4], 0),
// @Param: _IGN_ANG6
// @DisplayName: Proximity sensor ignore angle 6
// @Description: Proximity sensor ignore angle 6
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG6", 14, AP_Proximity, _ignore_angle_deg[5], 0),
// @Param: _IGN_WID6
// @DisplayName: Proximity sensor ignore width 6
// @Description: Proximity sensor ignore width 6
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID6", 15, AP_Proximity, _ignore_width_deg[5], 0),
#if PROXIMITY_MAX_INSTANCES > 1
// @Param: 2_TYPE
// @DisplayName: Second Proximity type
// @Description: What type of proximity sensor is connected
// @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder
// @User: Advanced
AP_GROUPINFO("2_TYPE", 16, AP_Proximity, _type[1], 0),
// @Param: 2_ORIENT
// @DisplayName: Second Proximity sensor orientation
// @Description: Second Proximity sensor orientation
// @Values: 0:Default,1:Upside Down
// @User: Standard
AP_GROUPINFO("2_ORIENT", 17, AP_Proximity, _orientation[1], 0),
// @Param: 2_YAW_CORR
// @DisplayName: Second Proximity sensor yaw correction
// @Description: Second Proximity sensor yaw correction
// @Units: degrees
// @Range: -180 180
// @User: Standard
AP_GROUPINFO("2_YAW_CORR", 18, AP_Proximity, _yaw_correction[1], PROXIMITY_YAW_CORRECTION_DEFAULT),
#endif
AP_GROUPEND
};
5》在missionplanner中显示参数,
4》》》》》》》》g2.proximity.set_rangefinder(&rangefinder);
// RangeFinder rangefinder {serial_manager, ROTATION_PITCH_270}; //测距使用
void set_rangefinder(const RangeFinder *rangefinder)
{
_rangefinder = rangefinder;
}
2.更新避障数据模块
SCHED_TASK(update_proximity, 100, 50), //更新近距离传感器,避障使用
1》》》》》》》》》》》》》》》》》》》分析代码
void Copter::update_proximity(void)
{
#if PROXIMITY_ENABLED == ENABLED
g2.proximity.update();
#endif
}
2》》》》》》》》》》》》分析代码g2.proximity.update();
void AP_Proximity::update(void)
{
for (uint8_t i=0; i<num_instances; i++)
{
if (drivers[i] != nullptr)
{
if (_type[i] == Proximity_Type_None)
{
//允许用户在运行时禁用接近传感器----- allow user to disable a proximity sensor at runtime
state[i].status = Proximity_NotConnected;
continue;
}
drivers[i]->update();//更新数据
}
}
//编制主实例-第一传感器返回良好数据---------work out primary instance - first sensor returning good data
for (int8_t i=num_instances-1; i>=0; i--)
{
if (drivers[i] != nullptr && (state[i].status == Proximity_Good))
{
primary_instance = i;
}
}
}
3》》》》》》》我们分析drivers[i]->update();//更新数据
//这里由于 virtual void update() = 0;是纯虚函数,我们以AP_Proximity_RangeFinder为例进行讲解
void AP_Proximity_RangeFinder::update(void)
{
//如果没有测距仪目标立即退出-------------exit immediately if no rangefinder object
const RangeFinder *rngfnd = frontend.get_rangefinder();
if (rngfnd == nullptr)
{
set_status(AP_Proximity::Proximity_NoData);
return;
}
//查看所有测距仪----------------------look through all rangefinders
for (uint8_t i=0; i<rngfnd->num_sensors(); i++)
{
if (rngfnd->has_data(i))
{
//检查水平测距仪------------check for horizontal range finders
if (rngfnd->get_orientation(i) <= ROTATION_YAW_315)
{
uint8_t sector = (uint8_t)rngfnd->get_orientation(i); //获取方向
_angle[sector] = sector * 45; //获取角度
_distance[sector] = rngfnd->distance_cm(i) / 100.0f; //获取距离
_distance_min = rngfnd->min_distance_cm(i) / 100.0f; //最小距离
_distance_max = rngfnd->max_distance_cm(i) / 100.0f; //最大距离
_distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max); //是否在限制的范围
_last_update_ms = AP_HAL::millis(); //获取上次时间
update_boundary_for_sector(sector);
}
//检查向上测距仪----------check upward facing range finder
if (rngfnd->get_orientation(i) == ROTATION_PITCH_90)
{
_distance_upward = rngfnd->distance_cm(i) / 100.0f;
_last_upward_update_ms = AP_HAL::millis();
}
}
}
//检查超时并设置健康状态--------- check for timeout and set health status
if ((_last_update_ms == 0) || (AP_HAL::millis() - _last_update_ms > PROXIMITY_RANGEFIDER_TIMEOUT_MS))
{
set_status(AP_Proximity::Proximity_NoData);
} else
{
set_status(AP_Proximity::Proximity_Good);
}
}
备注:这里要注意的是:这些值应该传到避障控制函数中,怎么传过去的。
3.避障控制初始化
《1》avoid_adsb_init(ignore_checks)
case AVOID_ADSB:
success = avoid_adsb_init(ignore_checks); //避障模块
break;
《2》bool Copter::avoid_adsb_init(const bool ignore_checks)
bool Copter::avoid_adsb_init(const bool ignore_checks)
{
//重用引导模式-----re-use guided mode
return guided_init(ignore_checks);
}
《3》bool Copter::guided_init(bool ignore_checks)
bool Copter::guided_init(bool ignore_checks)
{
if (position_ok() || ignore_checks)
{
//初始化偏航----------------------initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));//自动偏航设置
//开始在位置控制模式-------------start in position control mode
guided_pos_control_start();
return true;
}else
{
return false;
}
}
《3》.1
void Copter::guided_pos_control_start()
{
//设置为位置控制模式------- set to position control mode
guided_mode = Guided_WP;
//航路点与样条控制器的初始化----- initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
//将WPNav初始化为停止点---- initialise wpnav to stopping point
Vector3f stopping_point;
wp_nav->get_wp_stopping_point(stopping_point);
//不需要检查返回状态,因为不使用地形数据。---- no need to check return status because terrain data is not used
wp_nav->set_wp_destination(stopping_point, false);
//初始化偏航---- initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}
《3》.1-1 wp_nav->wp_and_spline_init();
void AC_WPNav::wp_and_spline_init()
{
// check _wp_accel_cms is reasonable
if (_wp_accel_cms <= 0)
{
_wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
}
// also limit the accel using the maximum lean angle. This
// prevents the navigation controller from trying to move the
// target point at an unachievable rate
float accel_limit_cms = GRAVITY_MSS * 100 * tanf(radians(_attitude_control.lean_angle_max()*0.01f));
if (_wp_accel_cms > accel_limit_cms)
{
_wp_accel_cms.set(accel_limit_cms);
}
//初始化位置控制器------------------initialise position controller
_pos_control.init_xy_controller();
_pos_control.clear_desired_velocity_ff_z();
//初始化位置控制器的速度和加速度-------initialise position controller speed and acceleration
_pos_control.set_speed_xy(_wp_speed_cms);
_pos_control.set_accel_xy(_wp_accel_cms);
_pos_control.set_jerk_xy_to_default();
_pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
_pos_control.set_accel_z(_wp_accel_z_cms);
_pos_control.calc_leash_length_xy();
_pos_control.calc_leash_length_z();
//将偏航航向初始化为当前航向目标---- initialise yaw heading to current heading target
_flags.wp_yaw_set = false;
}
《3》.2-1 void AC_WPNav::get_wp_stopping_point(Vector3f& stopping_point) const
void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
{
const Vector3f curr_pos = _inav.get_position();
Vector3f curr_vel = _inav.get_velocity();
float linear_distance; // the distance at which we swap from a linear to sqrt response
float linear_velocity; // the velocity above which we swap from a linear to sqrt response
float stopping_dist; // the distance within the vehicle can stop
float kP = _p_pos_xy.kP();
//增加速度误差到当前位置------add velocity error to current velocity
if (is_active_xy())
{
curr_vel.x += _vel_error.x;
curr_vel.y += _vel_error.y;
}
//计算当前速度矢量-------calculate current velocity
float vel_total = norm(curr_vel.x, curr_vel.y);
// avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
if (kP <= 0.0f || _accel_cms <= 0.0f || is_zero(vel_total))
{
stopping_point.x = curr_pos.x;
stopping_point.y = curr_pos.y;
return;
}
//计算速度从线性切换到SqRT的点----- calculate point at which velocity switches from linear to sqrt
linear_velocity = _accel_cms/kP;
//计算我们可以停止的距离---------------------- calculate distance within which we can stop
if (vel_total < linear_velocity)
{
stopping_dist = vel_total/kP;
} else
{
linear_distance = _accel_cms/(2.0f*kP*kP);
stopping_dist = linear_distance + (vel_total*vel_total)/(2.0f*_accel_cms);
}
//约束制动距离----------------------constrain stopping distance
stopping_dist = constrain_float(stopping_dist, 0, _leash);
//利用速度矢量将停车距离转换为停车点----convert the stopping distance into a stopping point using velocity vector
stopping_point.x = curr_pos.x + (stopping_dist * curr_vel.x / vel_total);
stopping_point.y = curr_pos.y + (stopping_dist * curr_vel.y / vel_total);
}
//计算垂直阻止点
void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
{
const float curr_pos_z = _inav.get_altitude();
float curr_vel_z = _inav.get_velocity_z();
float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt
float linear_velocity; // the velocity we swap between linear and sqrt
// if position controller is active add current velocity error to avoid sudden jump in acceleration
if (is_active_z()) {
curr_vel_z += _vel_error.z;
if (_flags.use_desvel_ff_z) {
curr_vel_z -= _vel_desired.z;
}
}
// avoid divide by zero by using current position if kP is very low or acceleration is zero
if (_p_pos_z.kP() <= 0.0f || _accel_z_cms <= 0.0f) {
stopping_point.z = curr_pos_z;
return;
}
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
linear_velocity = _accel_z_cms/_p_pos_z.kP();
if (fabsf(curr_vel_z) < linear_velocity) {
// if our current velocity is below the cross-over point we use a linear function
stopping_point.z = curr_pos_z + curr_vel_z/_p_pos_z.kP();
} else {
linear_distance = _accel_z_cms/(2.0f*_p_pos_z.kP()*_p_pos_z.kP());
if (curr_vel_z > 0){
stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
} else {
stopping_point.z = curr_pos_z - (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
}
}
stopping_point.z = constrain_float(stopping_point.z, curr_pos_z - POSCONTROL_STOPPING_DIST_DOWN_MAX, curr_pos_z + POSCONTROL_STOPPING_DIST_UP_MAX);
}
《3》.3 wp_nav->set_wp_destination(stopping_point, false);
bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
{
Vector3f origin;
// if waypoint controller is active use the existing position target as the origin
if ((AP_HAL::millis() - _wp_last_update) < 1000)
{
origin = _pos_control.get_pos_target();
} else
{
// if waypoint controller is not active, set origin to reasonable stopping point (using curr pos and velocity)
_pos_control.get_stopping_point_xy(origin);
_pos_control.get_stopping_point_z(origin);
}
//将原点转换为地形之上的ALT---- convert origin to alt-above-terrain
if (terrain_alt)
{
float origin_terr_offset;
if (!get_terrain_offset(origin_terr_offset))
{
return false;
}
origin.z -= origin_terr_offset;
}
//设置原点和目的地-------------- set origin and destination
return set_wp_origin_and_destination(origin, destination, terrain_alt);
}
4.避障控制更新
void Copter::update_flight_mode()往下调用
case AVOID_ADSB: //避障模式
avoid_adsb_run();
break;
《1》avoid_adsb_run();
void Copter::avoid_adsb_run()
{
// re-use guided mode's velocity controller
// Note: this is safe from interference from GCSs and companion computer's whose guided mode
// position and velocity requests will be ignored while the vehicle is not in guided mode
guided_run();
}
《2》guided_run()函数
void Copter::guided_run()
{
//调用正确的自动控制器------------------call the correct auto controller
switch (guided_mode)
{
case Guided_TakeOff:
//运行起飞控制器------------------run takeoff controller
guided_takeoff_run();
break;
case Guided_WP:
//运行位置控制器------------------run position controller
guided_pos_control_run();
break;
case Guided_Velocity:
//运行速度控制器------------------run velocity controller
guided_vel_control_run();
break;
case Guided_PosVel:
//运行位置速度控制器--------------run position-velocity controller
guided_posvel_control_run();
break;
case Guided_Angle:
//运行角度控制器------------------run angle controller
guided_angle_control_run();
break;
}
}
《2》-1起飞运行代码: guided_takeoff_run();
void Copter::guided_takeoff_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
// initialise wpnav targets
wp_nav->shift_wp_origin_to_current_pos();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// reset attitude control targets
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// clear i term when we're taking off
set_throttle_takeoff();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio)
{
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
#if FRAME_CONFIG == HELI_FRAME
// helicopters stay in landed state until rotor speed runup has finished
if (motors->rotor_runup_complete())
{
set_land_complete(false);
} else {
// initialise wpnav targets
wp_nav->shift_wp_origin_to_current_pos();
}
#else
set_land_complete(false);
#endif
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
// call attitude controller
auto_takeoff_attitude_run(target_yaw_rate);
}
《2》-2位置控制代码 guided_pos_control_run();
void Copter::guided_pos_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
failsafe_terrain_set_status(wp_nav->update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
// roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
}
}
《2》-3 速度控制代码:guided_vel_control_run();
void Copter::guided_vel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
// initialise velocity controller
pos_control->init_vel_controller_xyz();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero and stop rotating if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
if (!pos_control->get_desired_velocity().is_zero()) {
guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f));
}
if (auto_yaw_mode == AUTO_YAW_RATE) {
set_auto_yaw_rate(0.0f);
}
} else {
guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
}
// call velocity controller which includes z axis controller
pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
// roll & pitch from velocity controller, yaw rate from mavlink command or mission item
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
}
}
《2》-4位置控制代码: guided_posvel_control_run();
void Copter::guided_posvel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
// set target position and velocity to current position and velocity
pos_control->set_pos_target(inertial_nav.get_position());
pos_control->set_desired_velocity(Vector3f(0,0,0));
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// set velocity to zero and stop rotating if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
guided_vel_target_cms.zero();
if (auto_yaw_mode == AUTO_YAW_RATE) {
set_auto_yaw_rate(0.0f);
}
}
// calculate dt
float dt = pos_control->time_since_last_xy_update();
// update at poscontrol update rate
if (dt >= pos_control->get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// advance position target using velocity target
guided_pos_target_cm += guided_vel_target_cms * dt;
// send position and velocity targets to position controller
pos_control->set_pos_target(guided_pos_target_cm);
pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y);
// run position controller
pos_control->update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler, false);
}
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
// roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
}
}
《2》.5 guided_angle_control_run();
void Copter::guided_angle_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->set_yaw_target_to_current_heading();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
attitude_control->set_throttle_out(0.0f,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control->set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);
#endif
pos_control->relax_alt_hold_controllers(0.0f);
return;
}
// constrain desired lean angles
float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd;
float total_in = norm(roll_in, pitch_in);
float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), aparm.angle_max);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// wrap yaw request
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);
// constrain climb rate
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_speed_down()), wp_nav->get_speed_up());
// get avoidance adjusted climb rate
climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
roll_in = 0.0f;
pitch_in = 0.0f;
climb_rate_cms = 0.0f;
yaw_rate_in = 0.0f;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
if (guided_angle_state.use_yaw_rate) {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in, get_smoothing_gain());
} else {
attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
}
// call position controller
pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
pos_control->update_z_controller();
}