请选择 进入手机版 | 继续访问电脑版
正在上传图片(0/1)

F450机架搭载某开源飞控悬停定高试飞

 0
手机看帖 0 2948
欢迎各位大神出来交流交流哈..........

1.分析log文件,及其消息的赋值
    { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),  "CTUN", "Qhhfffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
void Copter::Log_Write_Control_Tuning()
{
    struct log_Control_Tuning pkt = {
        LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
        time_us             : AP_HAL::micros64(),
        throttle_in         : channel_throttle->control_in,
        angle_boost         : attitude_control.angle_boost(),
        throttle_out        : motors.get_throttle(),
        desired_alt         : pos_control.get_alt_target() / 100.0f,
        inav_alt            : inertial_nav.get_altitude() / 100.0f,
        baro_alt            : baro_alt,
        desired_sonar_alt   : (int16_t)target_sonar_alt,
        sonar_alt           : sonar_alt,
        desired_climb_rate  : (int16_t)pos_control.get_vel_target_z(),
        climb_rate          : climb_rate
    };
    DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
上面的结构体就是我们要分析定高效果时需要查看的部分主要信息,
time_us为运行时间,
throttle_in为油门输入,0到1000的整数值。
throttle_out油门输出,0到1000的float数值。
desired_alt目标高度, _pos_target.z的值除以100,单位为米。
inav_alt   飞机目前高度,_relpos_cm.z的值除以100,单位为米。
baro_alt 气压计的高度,单位为厘米。此值在sensors.cpp的read_barometer的函数中更新,read_barometer通过ArduCopter.cpp中的update_altitude调用,update_altitude为定义的10hz的schedule.
desired_climb_rate为需要的爬升速率,_vel_target.z的值,单位cm/s。
climb_rate为当前的爬升速率,此值在inertia.cpp中的read_inertial_altitude函数中更新,实际上就是读取的inertial_nav的_velocity_cm.z的值,read_inertial_altitude在ArduCopter.cpp中的throttle_loop函数中被调用,throttle_loop为定义的50hz的schedule.

2.我们来看相关定高函数
   当切换到定高模式时,althold_init先被运行,其代码如下:
bool Copter::althold_init(bool ignore_checks)
{
    // initialize vertical speeds and leash lengths
    //(-250,250),设置最大下降速率_speed_down_cms,最大上升速率_speed_up_cms,并重新计算了_leash_up_z,_leash_down_z。
    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);

   //250,设置最大加速度_accel_z_cms,并重新计  算了_leash_up_z,_leash_down_z。
    pos_control.set_accel_z(g.pilot_accel_z);

    // initialise position and desired velocity
    //将当前EKF的高度值(_relpos_cm.z)赋值给_pos_target.z。
    pos_control.set_alt_target(inertial_nav.get_altitude());
   
     //将当前EKF的爬升速率(_velocity_cm.z)赋值给_vel_desired.z。从这里可以看到,如果我们切到定高模式之前,飞机仍在爬升或下降,切换到定高模式后,飞机        还是会继续爬升或下降的
    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
    // stop takeoff if running
    takeoff_stop();
    return true;
}

高度控制函数在精简掉姿态控制相关内容后,代码如下:
void Copter::althold_run()
{
    AltHoldModeState althold_state;
    float takeoff_climb_rate = 0.0f;


    // initialize vertical speeds and acceleration
    // 设置z轴最小速度和最大速度
    pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
  // 设置z轴加速度
    pos_control.set_accel_z(g.pilot_accel_z);



    // get pilot desired climb rate
    // 根据遥控器输入,算出爬升速度
    float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->control_in);
    target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);


    // call position controller
    pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
    pos_control.update_z_controller();
}
在get_pilot_desired_climb_rate中需要获取mid_stick,注意由于THR_MIN和RC3_DZ的存在,这个值并不是500(千分之),而是548(futuba t8fg遥控器),所以deadband是448到648,要想保持定要,油门输入必须保持在448到648之间。
set_alt_target_from_climb_rate_ff是根据target_climb_rate的速度去计算目标高度,在这个函数内部用了一个取平方根的方式,使速度变化由快变慢,可以由下面matlab生成的图片看出来,x轴为此函数运行次数,y轴为_vel_desired.z,_vel_desired.z初始值为0,target_climb_rate设置100的时候画出的图,这样做的目的估计是使飞行器可以先快后慢的变化,保持最后的平稳。
   部分信息来源于网路。
评论
上传
你需要登录之后才能回帖    登录 | 注册
已累计飞行1816306米
认证设备
取消 点赞 评论
分享至:
回复:
上传
取消 评论
快速回复 返回顶部 返回列表