目录
摘要
**本文主要记录自己学习ardupilot的高度控制代码的过程。
使用的代码：Ardupilot_3.5.5，
编译环境：ubuntu14.04
看代码软件：eclipse/vscode/sourceinsight,我用的ubuntu**
1.高度控制器初始化
ardupilot要想进入定高模式，必须通过外部开关去映射，比如我们可以设置5通道中的一个地方作为定高，或者其他的通道也可以，这些都可以在地面站设置。设置好了之后，我们就是通过遥控器触发无人机进入定高模式（记住ardupilot不支持空中设置定高模式，所以我们要先设置定高模式，然后用遥控器触发进入定高模式），最后通过遥控器解锁开始在定高模式下飞行。
（1）地面站设定定高模式
（2）通过参数设置进入定高模式
因此我们看定高初始化代码，应该去找外部开关那里，因为这里就是进入定高的开始（设定飞行模式初始化）。
获取外部命令的函数，主要有两个
（1） SCHED_TASK(rc_loop, 100, 130), //遥控器数据读取函数，10ms
（2）SCHED_TASK(read_aux_switches, 10, 50), //读取遥控器外部开关，触发模式，100ms运行一次
后面对遥控器的代码专门去讲解，这里不在往下进行深入研究
主要关注上面两个调用set_mode的函数这个代码
这里先看下set_mode函数在哪里出现
void Copter::read_control_switch()
{
if (g.flight_mode_chan <= 0) {
// no flight mode channel
return;
}
uint32_t tnow_ms = millis();
// calculate position of flight mode switch
int8_t switch_position;
uint16_t mode_in = RC_Channels::rc_channel(g.flight_mode_chan-1)->get_radio_in();
if (mode_in < 1231) switch_position = 0;
else if (mode_in < 1361) switch_position = 1;
else if (mode_in < 1491) switch_position = 2;
else if (mode_in < 1621) switch_position = 3;
else if (mode_in < 1750) switch_position = 4;
else switch_position = 5;
// store time that switch last moved
if (control_switch_state.last_switch_position != switch_position) {
control_switch_state.last_edge_time_ms = tnow_ms;
}
// debounce switch
bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position;
bool sufficient_time_elapsed = tnow_ms - control_switch_state.last_edge_time_ms > CONTROL_SWITCH_DEBOUNCE_TIME_MS;
bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0;
if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged) {
// set flight mode and simple mode setting
if (set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND)) {
// play a tone
if (control_switch_state.debounced_switch_position != -1) {
// alert user to mode change failure (except if autopilot is just starting up)
if (ap.initialised) {
AP_Notify::events.user_mode_change = 1;
}
}
if (!check_if_auxsw_mode_used(AUXSW_SIMPLE_MODE) && !check_if_auxsw_mode_used(AUXSW_SUPERSIMPLE_MODE)) {
// if none of the Aux Switches are set to Simple or Super Simple Mode then
// set Simple Mode using stored parameters from EEPROM
if (BIT_IS_SET(g.super_simple, switch_position)) {
set_simple_mode(2);
} else {
set_simple_mode(BIT_IS_SET(g.simple_modes, switch_position));
}
}
} else if (control_switch_state.last_switch_position != -1) {
// alert user to mode change failure
AP_Notify::events.user_mode_change_failed = 1;
}
// set the debounced switch position
control_switch_state.debounced_switch_position = switch_position;
}
control_switch_state.last_switch_position = switch_position;
}
（1）第一处设置set_mode（）
if (set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND))（2）第二处设置set_mode（）
void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
{
switch(ch_function)
{
case AUXSW_FLIP:
// flip if switch is on, positive throttle and we're actually flying
if (ch_flag == AUX_SWITCH_HIGH)
{
set_mode(FLIP, MODE_REASON_TX_COMMAND);
}
break;
case AUXSW_SIMPLE_MODE:
// low = simple mode off, middle or high position turns simple mode on
set_simple_mode(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
break;
case AUXSW_SUPERSIMPLE_MODE:
// low = simple mode off, middle = simple mode, high = super simple mode
set_simple_mode(ch_flag);
break;
case AUXSW_RTL:
if (ch_flag == AUX_SWITCH_HIGH) {
// engage RTL (if not possible we remain in current flight mode)
set_mode(RTL, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in RTL
if (control_mode == RTL) {
reset_control_switch();
}
}
break;
case AUXSW_SAVE_TRIM:
if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_control_in() == 0)) {
save_trim();
}
break;
case AUXSW_SAVE_WP:
// save waypoint when switch is brought high
if (ch_flag == AUX_SWITCH_HIGH) {
// do not allow saving new waypoints while we're in auto or disarmed
if (control_mode == AUTO || !motors->armed()) {
return;
}
// do not allow saving the first waypoint with zero throttle
if ((mission.num_commands() == 0) && (channel_throttle->get_control_in() == 0)) {
return;
}
// create new mission command
AP_Mission::Mission_Command cmd = {};
// if the mission is empty save a takeoff command
if (mission.num_commands() == 0)
{
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
cmd.id = MAV_CMD_NAV_TAKEOFF;
cmd.content.location.options = 0;
cmd.p1 = 0;
cmd.content.location.lat = 0;
cmd.content.location.lng = 0;
cmd.content.location.alt = MAX(current_loc.alt,100);
// use the current altitude for the target alt for takeoff.
// only altitude will matter to the AP mission script for takeoff.
if (mission.add_cmd(cmd)) {
// log event
Log_Write_Event(DATA_SAVEWP_ADD_WP);
}
}
// set new waypoint to current location
cmd.content.location = current_loc;
// if throttle is above zero, create waypoint command
if (channel_throttle->get_control_in() > 0) {
cmd.id = MAV_CMD_NAV_WAYPOINT;
} else {
// with zero throttle, create LAND command
cmd.id = MAV_CMD_NAV_LAND;
}
// save command
if (mission.add_cmd(cmd)) {
// log event
Log_Write_Event(DATA_SAVEWP_ADD_WP);
}
}
break;
case AUXSW_CAMERA_TRIGGER:
#if CAMERA == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
do_take_picture();
}
#endif
break;
case AUXSW_RANGEFINDER:
// enable or disable the rangefinder
#if RANGEFINDER_ENABLED == ENABLED
if ((ch_flag == AUX_SWITCH_HIGH) && rangefinder.has_orientation(ROTATION_PITCH_270)) {
rangefinder_state.enabled = true;
} else {
rangefinder_state.enabled = false;
}
#endif
break;
case AUXSW_FENCE:
#if AC_FENCE == ENABLED
// enable or disable the fence
if (ch_flag == AUX_SWITCH_HIGH) {
fence.enable(true);
Log_Write_Event(DATA_FENCE_ENABLE);
} else {
fence.enable(false);
Log_Write_Event(DATA_FENCE_DISABLE);
}
#endif
break;
case AUXSW_ACRO_TRAINER:
switch(ch_flag) {
case AUX_SWITCH_LOW:
g.acro_trainer = ACRO_TRAINER_DISABLED;
Log_Write_Event(DATA_ACRO_TRAINER_DISABLED);
break;
case AUX_SWITCH_MIDDLE:
g.acro_trainer = ACRO_TRAINER_LEVELING;
Log_Write_Event(DATA_ACRO_TRAINER_LEVELING);
break;
case AUX_SWITCH_HIGH:
g.acro_trainer = ACRO_TRAINER_LIMITED;
Log_Write_Event(DATA_ACRO_TRAINER_LIMITED);
break;
}
break;
case AUXSW_GRIPPER:
#if GRIPPER_ENABLED == ENABLED
switch(ch_flag) {
case AUX_SWITCH_LOW:
g2.gripper.release();
Log_Write_Event(DATA_GRIPPER_RELEASE);
break;
case AUX_SWITCH_HIGH:
g2.gripper.grab();
Log_Write_Event(DATA_GRIPPER_GRAB);
break;
}
#endif
break;
case AUXSW_SPRAYER:
#if SPRAYER == ENABLED
sprayer.run(ch_flag == AUX_SWITCH_HIGH);
// if we are disarmed the pilot must want to test the pump
sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors->armed());
#endif
break;
case AUXSW_AUTO:
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(AUTO, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in AUTO
if (control_mode == AUTO) {
reset_control_switch();
}
}
break;
case AUXSW_AUTOTUNE:
#if AUTOTUNE_ENABLED == ENABLED
// turn on auto tuner
switch(ch_flag) {
case AUX_SWITCH_LOW:
case AUX_SWITCH_MIDDLE:
// restore flight mode based on flight mode switch position
if (control_mode == AUTOTUNE) {
reset_control_switch();
}
break;
case AUX_SWITCH_HIGH:
// start an autotuning session
set_mode(AUTOTUNE, MODE_REASON_TX_COMMAND);
break;
}
#endif
break;
case AUXSW_LAND:
if (ch_flag == AUX_SWITCH_HIGH)
{
set_mode(LAND, MODE_REASON_TX_COMMAND);
} else
{
// return to flight mode switch's flight mode if we are currently in LAND
if (control_mode == LAND) {
reset_control_switch();
}
}
break;
case AUXSW_PARACHUTE_ENABLE:
#if PARACHUTE == ENABLED
// Parachute enable/disable
parachute.enabled(ch_flag == AUX_SWITCH_HIGH);
#endif
break;
case AUXSW_PARACHUTE_RELEASE:
#if PARACHUTE == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
parachute_manual_release();
}
#endif
break;
case AUXSW_PARACHUTE_3POS:
#if PARACHUTE == ENABLED
// Parachute disable, enable, release with 3 position switch
switch (ch_flag) {
case AUX_SWITCH_LOW:
parachute.enabled(false);
Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case AUX_SWITCH_MIDDLE:
parachute.enabled(true);
Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case AUX_SWITCH_HIGH:
parachute.enabled(true);
parachute_manual_release();
break;
}
#endif
break;
case AUXSW_MISSION_RESET:
if (ch_flag == AUX_SWITCH_HIGH) {
mission.reset();
}
break;
case AUXSW_ATTCON_FEEDFWD:
// enable or disable feed forward
attitude_control->bf_feedforward(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_ATTCON_ACCEL_LIM:
// enable or disable accel limiting by restoring defaults
attitude_control->accel_limiting(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RETRACT_MOUNT:
#if MOUNT == ENABLE
switch (ch_flag) {
case AUX_SWITCH_HIGH:
camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT);
break;
case AUX_SWITCH_LOW:
camera_mount.set_mode_to_default();
break;
}
#endif
break;
case AUXSW_RELAY:
ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RELAY2:
ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RELAY3:
ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RELAY4:
ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_LANDING_GEAR:
switch (ch_flag) {
case AUX_SWITCH_LOW:
landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
break;
case AUX_SWITCH_HIGH:
landinggear.set_position(AP_LandingGear::LandingGear_Retract);
break;
}
break;
case AUXSW_LOST_COPTER_SOUND:
switch (ch_flag) {
case AUX_SWITCH_HIGH:
AP_Notify::flags.vehicle_lost = true;
break;
case AUX_SWITCH_LOW:
AP_Notify::flags.vehicle_lost = false;
break;
}
break;
case AUXSW_MOTOR_ESTOP:
// Turn on Emergency Stop logic when channel is high
set_motor_emergency_stop(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_MOTOR_INTERLOCK:
// Turn on when above LOW, because channel will also be used for speed
// control signal in tradheli
ap.motor_interlock_switch = (ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
break;
case AUXSW_BRAKE:
// brake flight mode
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(BRAKE, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in BRAKE
if (control_mode == BRAKE) {
reset_control_switch();
}
}
break;
case AUXSW_THROW:
// throw flight mode
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(THROW, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in throw mode
if (control_mode == THROW) {
reset_control_switch();
}
}
break;
case AUXSW_AVOID_ADSB:
// enable or disable AP_Avoidance
if (ch_flag == AUX_SWITCH_HIGH) {
avoidance_adsb.enable();
Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE);
} else {
avoidance_adsb.disable();
Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE);
}
break;
case AUXSW_PRECISION_LOITER:
#if PRECISION_LANDING == ENABLED
switch (ch_flag) {
case AUX_SWITCH_HIGH:
set_precision_loiter_enabled(true);
break;
case AUX_SWITCH_LOW:
set_precision_loiter_enabled(false);
break;
}
#endif
break;
case AUXSW_AVOID_PROXIMITY:
#if PROXIMITY_ENABLED == ENABLED && AC_AVOID_ENABLED == ENABLED
switch (ch_flag) {
case AUX_SWITCH_HIGH:
avoid.proximity_avoidance_enable(true);
Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_ENABLE);
break;
case AUX_SWITCH_LOW:
avoid.proximity_avoidance_enable(false);
Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_DISABLE);
break;
}
#endif
break;
case AUXSW_ARMDISARM:
// arm or disarm the vehicle
switch (ch_flag) {
case AUX_SWITCH_HIGH:
init_arm_motors(false);
break;
case AUX_SWITCH_LOW:
init_disarm_motors();
break;
}
break;
}
}
这里我们看下这个set_mode()函数的真面
bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
{
// boolean to record if flight mode could be set
bool success = false;
bool ignore_checks = !motors->armed(); //如果没有解锁，允许任何模式ignore_checks=1，_flags.armed=0表示没有解锁，_flags.armed=1表示解锁
// return immediately if we are already in the desired mode
if (mode == control_mode)
{
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode_reason = reason;
return true;
}
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter a non-manual throttle mode if the
// rotor runup is not complete
if (!ignore_checks && !mode_has_manual_throttle(mode) && !motors->rotor_runup_complete()){
goto failed;
}
#endif
switch (mode)
{
case ACRO:
#if FRAME_CONFIG == HELI_FRAME
success = heli_acro_init(ignore_checks);
#else
success = acro_init(ignore_checks);
#endif
break;
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
success = heli_stabilize_init(ignore_checks);
#else
success = stabilize_init(ignore_checks); //姿态自我稳定模式
#endif
break;
case ALT_HOLD:
success = althold_init(ignore_checks); //高度控制初始化
break;
case AUTO:
success = auto_init(ignore_checks); //自动模式
break;
case CIRCLE:
success = circle_init(ignore_checks);
break;
case LOITER:
success = loiter_init(ignore_checks); //定点悬停初始化
break;
case GUIDED:
success = guided_init(ignore_checks);
break;
case LAND:
success = land_init(ignore_checks);
break;
case RTL:
success = rtl_init(ignore_checks);
break;
case DRIFT:
success = drift_init(ignore_checks);
break;
case SPORT:
success = sport_init(ignore_checks);
break;
case FLIP:
success = flip_init(ignore_checks);
break;
#if AUTOTUNE_ENABLED == ENABLED
case AUTOTUNE:
success = autotune_init(ignore_checks);
break;
#endif
#if POSHOLD_ENABLED == ENABLED
case POSHOLD:
success = poshold_init(ignore_checks); //定点控制
break;
#endif
case BRAKE:
success = brake_init(ignore_checks);
break;
case THROW:
success = throw_init(ignore_checks);
break;
case AVOID_ADSB:
success = avoid_adsb_init(ignore_checks);
break;
case GUIDED_NOGPS:
success = guided_nogps_init(ignore_checks);
break;
default:
success = false;
break;
}
#if FRAME_CONFIG == HELI_FRAME
failed:
#endif
// update flight mode
if (success) {
// perform any cleanup required by previous flight mode
exit_mode(control_mode, mode);
prev_control_mode = control_mode;
prev_control_mode_reason = control_mode_reason;
control_mode = mode;
control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED));
#if AC_FENCE == ENABLED
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
// this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
// but it should be harmless to disable the fence temporarily in these situations as well
fence.manual_recovery_start();
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.update_control_mode(control_mode);
#endif
} else {
// Log error that we failed to enter desired flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode);
gcs_send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
}
// update notify object
if (success) {
notify_flight_mode(control_mode);
}
// return success or failure
return success;
}
看到了各种模式的初始化，我们今天重点关心定高的初始化
visio总结：
2.运行定高代码
void Copter::althold_run()
{
AltHoldModeState althold_state; //声明高度控制器模式变量
float takeoff_climb_rate = 0.0f; //设置爬升速率
//初始化垂直速度和加速度在一个范围--------------------------initialize vertical speeds and acceleration
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control->set_accel_z(g.pilot_accel_z);
//申请飞行模式是有头模式，还是无头模式到飞行器----------------apply SIMPLE mode transform to pilot inputs
update_simple_mode();
//获取飞行目标姿态角度------------------------------------get pilot desired lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->get_althold_lean_angle_max());
//获取飞行偏航角度---------------------------------------get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
//获取飞行爬升速率---------------------------------------get pilot desired climb rate
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
//限制爬升速度
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
#if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished
bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors->rotor_runup_complete());
#else
bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Alt Hold State Machine Determination
if (!motors->armed() || !motors->get_interlock()) //电机停转，安全开关关闭
{
althold_state = AltHold_MotorStopped;
}
else if (takeoff_state.running || takeoff_triggered) //起飞takeoff_state.running=1，才会起飞
{
althold_state = AltHold_Takeoff;
}
else if (!ap.auto_armed || ap.land_complete) //降落
{
althold_state = AltHold_Landed;
}
else //飞行
{
althold_state = AltHold_Flying;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 高度状态机控制
switch (althold_state)
{
case AltHold_MotorStopped:
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
#if FRAME_CONFIG == HELI_FRAME
//强制减弱控制-------------force descent rate and call position controller
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
heli_flags.init_targets_on_arming=true;
#else
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif
pos_control->update_z_controller();
break;
case AltHold_Takeoff: //起飞
#if FRAME_CONFIG == HELI_FRAME
if (heli_flags.init_targets_on_arming)
{
heli_flags.init_targets_on_arming=false;
}
#endif
//设置电机范围-------------------------set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
//初始化起飞变量-----------------------initiate take-off
if (!takeoff_state.running)
{
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
// indicate we are taking off
set_land_complete(false);
// clear i terms
set_throttle_takeoff();
}
//获取飞行棋调节和爬升率-----------------get take-off 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);
//计算姿态控制器-----------------------call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
//调用位置z控制------------------------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:
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f)
{
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
#if FRAME_CONFIG == HELI_FRAME
if (heli_flags.init_targets_on_arming)
{
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
if (motors->get_interlock())
{
heli_flags.init_targets_on_arming=false;
}
}
#else
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
#endif
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
break;
case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
#if AC_AVOID_ENABLED == ENABLED
// apply avoidance
avoid.adjust_roll_pitch(target_roll, target_pitch, aparm.angle_max);
#endif
//呼叫姿态控制器--------------------------call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
//利用测距仪调整爬升率---------------------adjust climb rate using rangefinder
if (rangefinder_alt_ok())
{
//如果测距仪可以，使用表面跟踪----------if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}
//获得避免调整爬升率----------------------get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
//调用位置控制器-------------------------call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control->update_z_controller(); //Z轴控制
break;
}
}
这里进行重点分析代码
上传一张总的图
(1)代码运行讲解
定高模式运行在哪里开始？
一点一点往下看代码
第一个变量：
AltHoldModeState althold_state; //声明高度控制器模式变量这个变量主要用在后面的飞行状态机
第二个变量：
float takeoff_climb_rate = 0.0f; //设置爬升速率，这个就是无人机的爬升速度
第三处代码分析：
//初始化垂直速度和加速度在一个范围--------------------------initialize vertical speeds and acceleration
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control->set_accel_z(g.pilot_accel_z);这里主要设置飞机的最大垂直速度和运行加速度，我们进入代码看下
/****************************************************************************************************************
*函数原型：void AC_PosControl::set_speed_z(float speed_down, float speed_up)
*函数功能：设置最大爬升速度和下降速度，要做到这一点：调用主代码飞行模式初始化函数
*修改日期：2018-5-26
*备 注：set_speed_z - sets maximum climb and descent rates
To-Do: call this in the main code as part of flight mode initialisation
calc_leash_length_z should be called afterwards
speed_down should be a negative number
*****************************************************************************************************************/
void AC_PosControl::set_speed_z(float speed_down, float speed_up)
{
//确保速度下降总是负值-------------- ensure speed_down is always negative
speed_down = -fabsf(speed_down);
if ((fabsf(_speed_down_cms-speed_down) > 1.0f) || (fabsf(_speed_up_cms-speed_up) > 1.0f))
{
_speed_down_cms = speed_down;
_speed_up_cms = speed_up;
_flags.recalc_leash_z = true;
calc_leash_length_z(); //计算速度模长
}
}/****************************************************************************************************************
*函数原型：void AC_PosControl::set_accel_z(float accel_cmss)
*函数功能：设置最大爬升速度和下降速度，要做到这一点：调用主代码飞行模式初始化函数
*修改日期：2018-5-26
*备 注：set_accel_z - set vertical acceleration in cm/s/s
*****************************************************************************************************************/
void AC_PosControl::set_accel_z(float accel_cmss)
{
if (fabsf(_accel_z_cms-accel_cmss) > 1.0f)
{
_accel_z_cms = accel_cmss;
_flags.recalc_leash_z = true;
calc_leash_length_z();
}
}
|
|
|
/************************************************************************************************************************************
*函数原型：void AC_PosControl::calc_leash_length_z()
*函数功能：计算垂直加速度模长
*修改日期：2018-6-8
*备 注：calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
called by pos_to_rate_z if z-axis speed or accelerations are changed
*************************************************************************************************************************************/
void AC_PosControl::calc_leash_length_z()
{
if (_flags.recalc_leash_z)
{
_leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_pos_z.kP());
_leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_pos_z.kP());
_flags.recalc_leash_z = false;
}
}
|
|
|
|
/************************************************************************************************************************************
*函数原型：float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const
*函数功能：计算给定最大速度、加速度和位置kp增益的水平皮带长度
*修改日期：2018-6-8
*备 注：calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
*************************************************************************************************************************************/
float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const
{
float leash_length;
//安全检查加速度----------sanity check acceleration and avoid divide by zero
if (accel_cms <= 0.0f)
{
accel_cms = POSCONTROL_ACCELERATION_MIN; //0.5m/s/s
}
// avoid divide by zero
if (kP <= 0.0f)
{
return POSCONTROL_LEASH_LENGTH_MIN;
}
// calculate leash length
if(speed_cms <= accel_cms / kP)
{
// linear leash length based on speed close in
leash_length = speed_cms / kP;
}else
{
// leash length grows at sqrt of speed further out
leash_length = (accel_cms / (2.0f*kP*kP)) + (speed_cms*speed_cms / (2.0f*accel_cms));
}
// ensure leash is at least 1m long
if( leash_length < POSCONTROL_LEASH_LENGTH_MIN )
{
leash_length = POSCONTROL_LEASH_LENGTH_MIN;
}
return leash_length;
}
**最后这个代码希望有人帮讲解下**第四处代码：
void Copter::update_simple_mode(void)
{
float rollx, pitchx;
//如果没有新遥控器数据，再简单模式下立即退出------exit immediately if no new radio frame or not in simple mode
if (ap.simple_mode == 0 || !ap.new_radio_frame) //没有配置简单模式就是直接正常模式
{
return;
}
//标记无线帧消耗---------- mark radio frame as consumed
ap.new_radio_frame = false;
//总结：简单模式就是飞行器起飞时头部对准的方向始终为机体坐标系的pitch轴，
//也就是说启动的时候就定死了机体坐标系，所以遥控器需要传入的 roll 和 pitch
//值需要转到机体坐标系，在转到地球坐标中。
if (ap.simple_mode == 1)//简单模式，头部对准的方向始终为机体方向
{
//旋转滚转，俯仰输入-初始简单航向（即面向北）---------rotate roll, pitch input by -initial simple heading (i.e. north facing)
rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw; //机体坐标
pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;
}else //无头模式
{
//旋转滚转，俯仰输入-超简单航向（倒向回家）---------rotate roll, pitch input by -super simple heading (reverse of heading to home)
rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw;
pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw;
}
// rotate roll, pitch input from north facing to vehicle's perspective
channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw()); //旋转到地理坐标
channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw()); //旋转到地理坐标
}这里主要设置无人机的遥控器操作无人机的模式：正常模式,简单模式，超简单模式
1》正常模式
总结：正常模式，无人机的机头指向哪里哪里就是正前方
2》简单模式
我们看下代码中的：
if (ap.simple_mode == 1)//简单模式，头部对准的方向始终为机体方向
{
//旋转滚转，俯仰输入-初始简单航向（即面向北）---------rotate roll, pitch input by -initial simple heading (i.e. north facing)
rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw; //机体坐标
pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;
}
channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw()); //旋转到地理坐标
channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw()); //旋转到地理坐标
3》超简单模式
else //无头模式
{
//旋转滚转，俯仰输入-超简单航向（倒向回家）---------rotate roll, pitch input by -super simple heading (reverse of heading to home)
rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw;
pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw;
}
// rotate roll, pitch input from north facing to vehicle's perspective
channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw()); //旋转到地理坐标
channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw()); //旋转到地理坐标
calc_distance_and_bearing(); //计算朝向
——————————————————————————————————————————————》
void Copter::calc_distance_and_bearing()
{
calc_wp_distance();
calc_wp_bearing();
calc_home_distance_and_bearing();
}
——————————————————————————————————————————————--》
void Copter::calc_home_distance_and_bearing()
{
// calculate home distance and bearing
if (position_ok()) {
Vector3f home = pv_location_to_vector(ahrs.get_home());
Vector3f curr = inertial_nav.get_position();
home_distance = pv_get_horizontal_distance_cm(curr, home);
home_bearing = pv_get_bearing_cd(curr,home);
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_bearing(false);
}
}
————————————————————————————————————————————————————————》
void Copter::update_super_simple_bearing(bool force_update)
{
// check if we are in super simple mode and at least 10m from home
if(force_update || (ap.simple_mode == 2 && home_distance > SUPER_SIMPLE_RADIUS))
{
// check the bearing to home has changed by at least 5 degrees
if (labs(super_simple_last_bearing - home_bearing) > 500)
{
super_simple_last_bearing = home_bearing;
float angle_rad = radians((super_simple_last_bearing+18000)/100);
super_simple_cos_yaw = cosf(angle_rad);
super_simple_sin_yaw = sinf(angle_rad);
}
}
}
第五处代码：获取目标姿态角和爬升率
//获取飞行目标姿态角度------------------------------------get pilot desired lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->get_althold_lean_angle_max());
//获取飞行偏航角度---------------------------------------get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
//获取飞行爬升速率---------------------------------------get pilot desired climb rate
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
//限制爬升速度
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
//安全检查角最大参数------------ sanity check angle max parameter
aparm.angle_max = constrain_int16(aparm.angle_max,1000,8000);
//限制最大倾斜角度-------------limit max lean angle
angle_max = constrain_float(angle_max, 1000, aparm.angle_max);
//求出系数--------------------scale roll_in, pitch_in to ANGLE_MAX parameter range
float scaler = aparm.angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX; ///4500
roll_in *= scaler;
pitch_in *= scaler;
//循环极限--------------------do circular limit
float total_in = norm(pitch_in, roll_in); //横滚和俯仰合成限制
if (total_in > angle_max)
{
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// do lateral tilt to euler roll conversion
roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in*(M_PI/18000)));
// return
roll_out = roll_in;
pitch_out = pitch_in;
}
float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
float yaw_request;
// calculate yaw rate request
if (g2.acro_y_expo <= 0)
{
yaw_request = stick_angle * g.acro_yaw_p; //g.acro_yaw_p=4.5
} else
{
// expo variables
float y_in, y_in3, y_out;
// range check expo
if (g2.acro_y_expo > 1.0f || g2.acro_y_expo < 0.5f)
{
g2.acro_y_expo = 1.0f;
}
// yaw expo
y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;
y_in3 = y_in*y_in*y_in;
y_out = (g2.acro_y_expo * y_in3) + ((1.0f - g2.acro_y_expo) * y_in);
yaw_request = ROLL_PITCH_YAW_INPUT_MAX * y_out * g.acro_yaw_p;
}
// convert pilot input to the desired yaw rate
return yaw_request;
}
float Copter::get_pilot_desired_climb_rate(float throttle_control)
{
// throttle failsafe check
if( failsafe.radio ) {
return 0.0f;
}
float desired_rate = 0.0f;
float mid_stick = channel_throttle->get_control_mid();
float deadband_top = mid_stick + g.throttle_deadzone;
float deadband_bottom = mid_stick - g.throttle_deadzone;
// ensure a reasonable throttle value
throttle_control = constrain_float(throttle_control,0.0f,1000.0f);
// ensure a reasonable deadzone
g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);
// check throttle is above, below or in the deadband
if (throttle_control < deadband_bottom) {
// below the deadband
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / deadband_bottom;
}else if (throttle_control > deadband_top) {
// above the deadband
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_top) / (1000.0f-deadband_top);
}else{
// must be in the deadband
desired_rate = 0.0f;
}
return desired_rate;
}第六处代码：获取定高状态机
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//定高模式状态机选择-------------Alt Hold State Machine Determination
if (!motors->armed() || !motors->get_interlock()) //电机停转，安全开关关闭
{
althold_state = AltHold_MotorStopped;
}
else if (takeoff_state.running || takeoff_triggered) //起飞takeoff_state.running=1，才会起飞
{
althold_state = AltHold_Takeoff;
}
else if (!ap.auto_armed || ap.land_complete) //降落
{
althold_state = AltHold_Landed;
}
else //飞行
{
althold_state = AltHold_Flying;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////第七处代码：运行定高状态机
// 高度状态机控制
switch (althold_state)
{
case AltHold_MotorStopped:
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
#if FRAME_CONFIG == HELI_FRAME
//强制减弱控制-------------force descent rate and call position controller
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
heli_flags.init_targets_on_arming=true;
#else
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif
pos_control->update_z_controller();
break;
case AltHold_Takeoff: //起飞
#if FRAME_CONFIG == HELI_FRAME
if (heli_flags.init_targets_on_arming)
{
heli_flags.init_targets_on_arming=false;
}
#endif
//设置电机范围-------------------------set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
//初始化起飞变量-----------------------initiate take-off
if (!takeoff_state.running)
{
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
// indicate we are taking off
set_land_complete(false);
// clear i terms
set_throttle_takeoff();
}
//获取飞行棋调节和爬升率-----------------get take-off 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);
//计算姿态控制器-----------------------call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
//调用位置z控制------------------------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:
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f)
{
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
#if FRAME_CONFIG == HELI_FRAME
if (heli_flags.init_targets_on_arming)
{
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
if (motors->get_interlock())
{
heli_flags.init_targets_on_arming=false;
}
}
#else
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
#endif
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
break;
case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
#if AC_AVOID_ENABLED == ENABLED
// apply avoidance
avoid.adjust_roll_pitch(target_roll, target_pitch, aparm.angle_max);
#endif第八处代码：运行姿态控制器
//呼叫姿态控制器--------------------------call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
第九处代码：计算爬升速度
//利用测距仪调整爬升率---------------------adjust climb rate using rangefinder
if (rangefinder_alt_ok())
{
//如果测距仪可以，使用表面跟踪----------if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}
//获得避免调整爬升率----------------------get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
//调用位置控制器-------------------------call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);第十处代码：调运Z轴控制器
pos_control->update_z_controller(); //Z轴控制
1》函数解析1
2》函数解析2
void AC_PosControl::calc_leash_length_z()
{
if (_flags.recalc_leash_z)
{
_leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_pos_z.kP());
_leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_pos_z.kP());
_flags.recalc_leash_z = false;
}
}3》函数解析3
void AC_PosControl::pos_to_rate_z()
{
float curr_alt = _inav.get_altitude(); //获取导航高度
//清除位置限制标志----------------------------------------clear position limit flags
_limit.pos_up = false;
_limit.pos_down = false;
//计算位置误差--------------------------------------------calculate altitude error
_pos_error.z = _pos_target.z - curr_alt;
//不要让目标高度离当前高度太远---------------------------------do not let target altitude get too far from current altitude
if (_pos_error.z > _leash_up_z)
{
_pos_target.z = curr_alt + _leash_up_z;
_pos_error.z = _leash_up_z;
_limit.pos_up = true;
}
if (_pos_error.z < -_leash_down_z)
{
_pos_target.z = curr_alt - _leash_down_z;
_pos_error.z = -_leash_down_z;
_limit.pos_down = true;
}
//计算目标速度，通过Z轴控制器和平方根控制器-------------------------calculate _vel_target.z using from _pos_error.z using sqrt controller
_vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms);
//速度限制-----------------------------------------------------check speed limits
// To-Do: check these speed limits here or in the pos->rate controller
_limit.vel_up = false;
_limit.vel_down = false;
if (_vel_target.z < _speed_down_cms)
{
_vel_target.z = _speed_down_cms;
_limit.vel_down = true;
}
if (_vel_target.z > _speed_up_cms)
{
_vel_target.z = _speed_up_cms;
_limit.vel_up = true;
}
//添加前馈组件----------------------------------------------------add feed forward component
if (_flags.use_desvel_ff_z)
{
_vel_target.z += _vel_desired.z;
}
//调用油门速率控制器，它将根据加速度的变化进行调节--call rate based throttle controller which will update accel based throttle controller targets
rate_to_accel_z();
}这段代码是我们关注的重点：
1》》获取导航高度和目标高度，计算垂直PID控制的目标误差输入
_pos_error.z = _pos_target.z - curr_alt;//获取垂直PID误差
其中float curr_alt = _inav.get_altitude(); //获取实际的导航高度，这个值来自EKF处理后的数据，
然而实际的目标高度来自油门值：
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
{
//计算增加的最大加速度--------------calculated increased maximum acceleration if over speed
float accel_z_cms = _accel_z_cms;
if (_vel_desired.z < _speed_down_cms && !is_zero(_speed_down_cms))
{
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms;
}
if (_vel_desired.z > _speed_up_cms && !is_zero(_speed_up_cms))
{
accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_up_cms;
}
accel_z_cms = constrain_float(accel_z_cms, 0.0f, 750.0f);
// jerk_z is calculated to reach full acceleration in 1000ms.
float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;
float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z));
_accel_last_z_cms += jerk_z * dt;
_accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);
float vel_change_limit = _accel_last_z_cms * dt;
_vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
_flags.use_desvel_ff_z = true;
// adjust desired alt if motors have not hit their limits
// To-Do: add check of _limit.pos_down?
if ((_vel_desired.z<0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
_pos_target.z += _vel_desired.z * dt;
}
}这里关键还是得找到油门大小怎么控制高度的？
//获取油门值大小函数
float Copter::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
{
if (thr_mid <= 0.0f)
{
thr_mid = motors->get_throttle_hover();
}
int16_t mid_stick = channel_throttle->get_control_mid();
// protect against unlikely divide by zero
if (mid_stick <= 0)
{
mid_stick = 500;
}
// ensure reasonable throttle values
throttle_control = constrain_int16(throttle_control,0,1000); //确保数据值在0-1000
// calculate normalised throttle input
float throttle_in;
if (throttle_control < mid_stick)
{
// below the deadband
throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
}else if(throttle_control > mid_stick)
{
// above the deadband
throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick);
}else
{
// must be in the deadband
throttle_in = 0.5f;
}
float expo = constrain_float(-(thr_mid-0.5)/0.375, -0.5f, 1.0f);
// calculate the output throttle using the given expo function
float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
return throttle_out;
}油门大小怎么控制高度的？
//获取飞行爬升速率---------------------------------------get pilot desired climb rate
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
//限制爬升速度
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);到这里我们看到油门值变化大小主要转换成控制无人机的爬升速度，也就是你控制摇杆大小实际控制的是无人机的爬升速度。，通过爬升速度加上实际位置的反馈作为目标高度，然后与实际的EKF获取的导航速度进行运算，得到高度误差，然后进行PID运算，得到控制量，进而转换成电机需要的PWM值，来实现控制无人机高度。
那么我们看下这个无人机怎么设定定高点的：
float Copter::get_pilot_desired_climb_rate(float throttle_control)
{
// throttle failsafe check
if( failsafe.radio )
{
return 0.0f;
}
float desired_rate = 0.0f;
float mid_stick = channel_throttle->get_control_mid();
float deadband_top = mid_stick + g.throttle_deadzone;
float deadband_bottom = mid_stick - g.throttle_deadzone;
//确保一个合理的油门值-----------ensure a reasonable throttle value
throttle_control = constrain_float(throttle_control,0.0f,1000.0f);
//进行死区限制--------------------ensure a reasonable deadzone
g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);
//检查油门是否在死区的上方或者下方----------check throttle is above, below or in the deadband
if (throttle_control < deadband_bottom)
{
//死区以下---------below the deadband
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / deadband_bottom;
}else if (throttle_control > deadband_top) {
//死区以上----------above the deadband
desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_top) / (1000.0f-deadband_top);
}else
{
//定高一定要在死区-----must be in the deadband
desired_rate = 0.0f;
}
return desired_rate; //得到爬升速率
}这里为了更好的理解，用visio画个图看下
这里不继续往下分析，我们将在定点模式下，继续分析！！！
这里我之前移植过baseflight的定高效果还可以，具体定高流程欢迎看我的论文baseflight定高分析
下面我们看下天穹无人机的定高代码：
（1）定高控制代码
static void AltControl(RCCOMMAND_t rcCommand)
{
static int32_t lastTimeAltChanged = 0;
static int16_t rcDeadband = 100;
static float speedUpRate = (float)ALT_SPEED_UP_MAX / MAXRCDATA;
static float speedDownRate = (float)ALT_SPEED_DOWN_MAX / MAXRCDATA;
static uint8_t altHoldChanged = 0;
static float velCtlTarget = 0;
/**********************************************************************************************************
高度控制：该模式下油门摇杆量控制上升下降速度，回中时飞机自动定高
**********************************************************************************************************/
rcCommand.throttle = (rcCommand.throttle - 1000) * 0.5f;
if (abs(rcCommand.throttle) > rcDeadband)
{
rcCommand.throttle = ApplyDeadbandInt((rcCommand.throttle), rcDeadband);
//摇杆量转为目标速度，低通滤波改变操控手感
if(rcCommand.throttle > 0)
{
velCtlTarget = velCtlTarget * 0.95f + (rcCommand.throttle * speedUpRate) * 0.05f;
}
else
velCtlTarget = velCtlTarget * 0.95f + (rcCommand.throttle * speedDownRate) * 0.05f;
//直接控制速度，禁用高度控制
SetAltCtlStatus(DISABLE);
//更新高度控制目标
posCtlTarget.z = GetCopterPosition().z;
//更新高度控制状态
SetAltControlStatus(ALT_CHANGED);
altHoldChanged = 1;
lastTimeAltChanged = GetSysTimeMs();
}
else if(altHoldChanged)
{
if(GetAltControlStatus() == ALT_CHANGED)
{
velCtlTarget = GetCopterVelocity().z;
//更新高度控制状态
SetAltControlStatus(ALT_CHANGED_FINISH);
}
else
{
//油门回中后先缓冲一段时间再进入定高
if(GetSysTimeMs() - lastTimeAltChanged < 1000)
{
velCtlTarget -= velCtlTarget * 0.08f;
}
else
{
altHoldChanged = 0;
}
//更新高度控制目标
posCtlTarget.z = GetCopterPosition().z;
}
}
else
{
//使能高度控制
SetAltCtlStatus(ENABLE);
//更新高度控制状态
SetAltControlStatus(ALT_HOLD);
}
//更新高度内环控制目标
SetAltInnerCtlTarget(velCtlTarget);
//更新高度外环控制目标
SetAltOuterCtlTarget(posCtlTarget.z);
}具体简单分析下：1》首先获取油门的值，2》判断遥杆的范围值，这里分二种情况，
第一：摇杆在死区以外，
获取定高目标速度,摇杆空速度
第二摇杆不变化，进入定高保持
（2）外环PID控制
（3）内环PID控制
fc.altInnerCtlValue = AltitudeInnerControl(GetCopterVelocity().z, deltaT);static float AltitudeInnerControl(float velZ, float deltaT)
{
static float velLpf;
float altInnerControlOutput;
//悬停油门中点
int16_t throttleMid = 1000;
/****************************************************************************************
目前高度控制由高度环P控制以及速度环PID控制串联而成
速度环的D项，即加速度，起到速度前馈控制的效果，但速度微分过程会放大数据噪声，导致控制效果有限
后续考虑将加速度测量值代替速度环D项的微分值，提升前馈效果
但本身加速度测量值的噪声也比较大，可能需要使用一个合适的低通滤波来减少数据噪声
*****************************************************************************************/
//限制待机状态下的油门输出
if(GetFlightStatus() == STANDBY)
fc.posInnerTarget.z = -150;
//对速度测量值进行低通滤波，减少数据噪声对控制器的影响
velLpf = velLpf * 0.95f + velZ * 0.05f;
//计算控制误差
fc.posInnerError.z = fc.posInnerTarget.z - velLpf;
//PID算法，计算出高度内环（Z轴速度）的控制量
altInnerControlOutput = PID_GetPI(&fc.pid[VEL_Z], fc.posInnerError.z, deltaT);
altInnerControlOutput += PID_GetD(&fc.pid[VEL_Z], fc.posInnerError.z, deltaT);
//在PID控制量上加入油门前馈补偿
altInnerControlOutput += throttleMid;
//油门输出限幅
altInnerControlOutput = ConstrainFloat(altInnerControlOutput, 200, 1800);
return altInnerControlOutput;
} 这个代码这里不进行详细分析，感兴趣的可以看群主的代码：天穹开源飞控
这里继续开始将apm代码，高度三级PID串级控制
2》》高度外环P控制，得到速度环目标控制量
_vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms);
具体看下代码
3》》速度环P控制
4》》加速度环PID控制
得到加速度环的控制输出量：
float thr_out = (p+i+d)/1000.0f +_motors.get_throttle_hover();
把数据传送到油门控制，之间摇杆可以控制电机转动驱动的PWM的大小
_attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);/**************************************************************************************************************************
*函数原型：void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
*函数功能：设置油门输出到电机
*修改日期：2018-6-8
*备 注：
***************************************************************************************************************************/
void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
{
_throttle_in = throttle_in;
update_althold_lean_angle_max(throttle_in);
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (apply_angle_boost)
{
//应用角度提升———————Apply angle boost
throttle_in = get_throttle_boosted(throttle_in);
}else
{
//用于测井目的的清晰角度提升———Clear angle_boost for logging purposes
_angle_boost = 0.0f;
}
_motors.set_throttle(throttle_in);
_motors.set_throttle_avg_max(get_throttle_avg_max(MAX(throttle_in, _throttle_in)));
}
如果感觉写的不错，给点个赞，你的支持是我最大的动力！九天揽月带你玩转Ardupilot飞控代码！！！
欢迎关注公众号：
