Plane.Sdk3/PlaneGcsSdk_Shared/CopterControllers/GuidController.cs

using System;
using System.Collections.Generic;
using System.Text;

namespace Plane.CopterControllers
{
    class GuidController
    {

        float fabsf(float vvalue)
        {
            return Math.Abs(vvalue);

        }
        float sqrt(float vvalue)
        {
            return (float)Math.Sqrt(vvalue);

        }


        /// <summary>
        /// 计算小航点飞行
        /// </summary>
        /// <param name="Distance"></param>
        /// <param name="fc_acc"></param>
        /// <param name="fc_maxspeed"></param>
        /// <returns></returns>

        //单算减速，不算加速的最小飞行时间
        float getMinfligthtime_v2(float Distance, float fc_acc, float fc_maxspeed)
        {
            float fDis = fabsf(Distance); // 总距离
            float facc = fabsf(fc_acc);   // 减速度

            // 物体开始时即以最大速度运动，不考虑加速过程
            float vel = fc_maxspeed;

            // 计算减速所需的时间和距离
            // 减速时间 (从最大速度减速到0)
            float dectime = vel / facc;
            // 减速阶段覆盖的距离
            float decdis = (vel * dectime) - (0.5f * facc * dectime * dectime);

            // 判断是否有足够的距离进行减速
            if (decdis >= fDis)
            {
                // 如果减速所需距离已经超过或等于总距离，调整最大速度
                // 使用公式 v^2 = u^2 + 2as 解出 v
                vel = (float)Math.Sqrt(2 * facc * fDis);
                // 重新计算减速时间
                dectime = vel / facc;
            }

            // 计算匀速阶段时间
            float unftime = 0.0f;
            if (decdis < fDis)
            {
                // 如果有剩余距离进行匀速运动
                unftime = (fDis - decdis) / vel;
            }

            // 总飞行时间 = 匀速阶段时间 + 减速阶段时间
            return unftime + dectime;
        }

        //单算减速，不算加速的最大飞行速度
        public float CalculateMaximumVelocity(float distance, float time, float acceleration)
        {
            // 二次方程系数
            float a_coeff = 1;
            float b_coeff = -2 * time;
            float c_coeff = (2 * distance) / acceleration;

            // 计算判别式
            float discriminant = b_coeff * b_coeff - 4 * a_coeff * c_coeff;

            if (discriminant < 0)
            {
                return -1; // 无实数解
            }

            // 计算二次方程的根
            float t1_root1 = (-b_coeff + (float)Math.Sqrt(discriminant)) / (2 * a_coeff);
            float t1_root2 = (-b_coeff - (float)Math.Sqrt(discriminant)) / (2 * a_coeff);

            // 选择合理的根作为 t1
            float t1 = Math.Min(t1_root1, t1_root2);
            if (t1 < 0 || t1 > time)
            {
                return -1; // 无合理解
            }

            // 计算最大速度 v
            return acceleration * t1;
        }

        //单算加速，不算减速的距离速度计算
         float getspeeddist_V2(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
        {
            vdist = 0;
            vspeed = 0;
            float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
            if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
            {
                vspeed = Distance / flight_time;
                vdist = vspeed * curr_time;  //匀速距离
                return 0;

            }
            float dect = desV / fc_acc;  // 总加速时间
            float unit = flight_time - dect; //匀速段时间
            float decD = (fc_acc * dect * dect) / 2;   //总加速距离

            if (dect > flight_time) dect = flight_time;//约束时间
            if (curr_time > dect)  //大于加速段时间--匀速
            {
                vspeed = (Distance - decD) / (flight_time - dect);
                vdist = vspeed * (curr_time - dect);
                vdist = vdist + decD; //总距离=匀速距离+减速距离
                if (vdist > Distance) vdist = Distance; //约束距离

            }
            else  //加速段
            {
                vspeed = fc_acc * curr_time;
                vdist = (fc_acc * curr_time * curr_time) / 2;   //加速距离
            }
            return 0;
        }

        //单算减速，不算加速的距离速度计算
         float getspeeddist_V3(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
        {
            vdist = 0;
            vspeed = 0;
            float desV = CalculateMaximumVelocity(Distance, flight_time, fc_acc);
            if ((desV == -1) || (desV > fc_maxspeed)) //飞不到
            {
                vspeed = Distance / flight_time;
                vdist = vspeed * curr_time;  //匀速距离
                return 0;

            }
            float dect = desV / fc_acc;  // 总减速时间
            float unit = flight_time - dect; //匀速段时间
            float decD = desV * dect - (fc_acc * dect * dect) / 2;   //总减速距离

            if (dect > flight_time) dect = flight_time;//约束时间
                                                       //匀速时间内
            if (curr_time < unit)
            {
                vspeed = (Distance - decD) / (flight_time - dect);
                vdist = vspeed * curr_time;
            }
            else
            {
                if (((flight_time - dect) * unit) == 0)
                    vdist = 0;
                else
                    vdist = (Distance - decD) / (flight_time - dect) * unit;  //匀速距离
                float currdect = curr_time - unit;  //减速时间
                if (currdect >= 0)
                {
                    vspeed = desV - fc_acc * currdect;
                    decD = desV * currdect - (fc_acc * currdect * currdect) / 2;   //减速距离
                }
                vdist = vdist + decD; //总距离=匀速距离+减速距离
                if (vdist > Distance) vdist = Distance; //约束距离
            }
            return 0;
        }



         float getMinfligthtime(float Distance, float fc_acc, float fc_maxspeed)
        {
            float fDis = fabsf(Distance);
            float facc = fabsf(fc_acc);

            float realflytime = 0.0f;
            //计算一半的距离
            float hafdis = (fDis / 2);
            //计算最大速度
            float vel = (float)sqrt(2 * facc * hafdis);
            //如果速度超过最大速度
            if (vel > fc_maxspeed)
                //使用最大速度
                vel = fc_maxspeed;
            //加速时间
            float acctim = vel / facc;
            //加速距离
            float accdis = vel * vel / (2 * facc);
            //匀速段时间
            float vtime = (hafdis - accdis) / vel;
            //到一半的时间
            float haftime = acctim + vtime;
            realflytime = haftime * 2;
            return realflytime;
        }

        ///计算飞行距离和速度   单位--米，秒----
        //返回 整个距离最大飞行速度
        ///flight_time 总飞行时间  Distance 飞行总距离  curr_time 当前飞行时间  fc_acc加速度, fc_maxspeed最大速度  vdist 计算的应该飞的距离  vspeed 计算的当前时间应该速度
        //========这是飞控正在使用的算法========
         float getspeeddist_V1(float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
        {
            float plan_flytime=_plan_flytime;
            float desired_velocity=_desired_velocity;
            float dist = 0;
            //导航加速度 米/秒*秒  ---不使用导航库
            float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5; 
            //
            float speed = 0;
            //计算实时速度
            if (curr_time <= (plan_flytime / 2.0))
                speed = curr_time * wpacc;
            else
                //需要减速
                speed = (plan_flytime - curr_time) * wpacc;
            //不能大于目标速度
            if (speed > desired_velocity)
                speed = desired_velocity;
            if (speed <= 0)
                speed = 0;
            vspeed = speed;
            //计算实时距离
            float V_start = 0.0f;
            float V_end = 0.0f;
            //加速段
            float t1 = (desired_velocity - V_start) / wpacc;
            //减速段
            float t3 = (desired_velocity - V_end) / wpacc;
            //平飞段
            float t2 = plan_flytime - t1 - t3;
            if (curr_time < t1)
            {
                dist = 0.5f * wpacc * curr_time * curr_time + curr_time * V_start;
            }
            else if (curr_time < t1 + t2)
            {
                dist = -0.5f * wpacc * t1 * t1 + (curr_time) * desired_velocity;
            }
            else
            {
                float t = curr_time - t1 - t2;
                dist = -0.5f * wpacc * t1 * t1 + curr_time * desired_velocity - 0.5f * wpacc * t * t;
            }

            if (fabsf(dist) > fabsf(Distance))
                vdist = Distance;
            else
            {
                if (Distance < 0)
                    vdist = -dist;
                else vdist = dist;
            }

            return desired_velocity;
        }

         float _vspeed=0;

         float _desired_velocity = 0;
         float _plan_flytime = 0;

         float initgetspeeddist_V1(float flight_time, float Distance, float fc_acc, float fc_maxspeed)
        {
            //计划飞行时间
            float plan_flytime = flight_time;
            //计算距离用绝对值
            float fDis = fabsf(Distance);

            //导航加速度 米/秒*秒  ---不使用导航库
            float wpacc = fabsf(fc_acc);// g.fc_acc_xy;//1.5; 

            //最大目标速度---米/秒
            float desired_velocity = 0;


            //计算最小飞行时间
            float minflytime = getMinfligthtime(Distance, fc_acc, fc_maxspeed);
            if (flight_time < minflytime)
                plan_flytime = minflytime;// + 0.1f;


            //根据总飞行时间和总距离计算飞行速度----起始和结束速度都是0，中间按匀加速和匀减速计算，没考虑加加速度
            float delta = (0.5f * plan_flytime) * (0.5f * plan_flytime) - fDis / wpacc;
            if (delta >= 0)
            {
                desired_velocity = (0.5f * plan_flytime - sqrt(delta)) / (1 / wpacc);
                if (desired_velocity > fc_maxspeed)
                {
                    plan_flytime = minflytime;
                    desired_velocity = fc_maxspeed;
                }
            }
            else
            {
                desired_velocity = (0.5f * plan_flytime) / (1 / wpacc);
            }
            if (desired_velocity < 0.2f) desired_velocity = 0.2f;       //限制最小速度0.2米/秒
            if (desired_velocity > fc_maxspeed) desired_velocity = fc_maxspeed;//限制最大速度10米/秒，应该参数化

            _desired_velocity = desired_velocity;
            _plan_flytime = plan_flytime;
            return desired_velocity;
        }


        public  float initgetspeeddist(int flytype, float flight_time, float Distance, float fc_acc, float fc_maxspeed)
        {
            flytype = 0;
            switch (flytype)
            {
                case 0: //匀速 
                    _vspeed = Distance / flight_time;
                    return 0;
                case 1: //同时计算加减速 
                    return initgetspeeddist_V1(flight_time, Distance, fc_acc, fc_maxspeed);
                case 2:  //单加速 
                    return 0;
                case 3: //单减速
                    return 0;
                default:
                 
                    return 0;
            }

        }
        public  float getspeeddist(int flytype, float flight_time, float Distance, float curr_time, float fc_acc, float fc_maxspeed, out float vdist, out float vspeed)
        {
            flytype = 0;
            switch (flytype)
            {
                case 0: //匀速 
                //case 1: //匀速 
                    vspeed = _vspeed;
                    vdist = vspeed * curr_time;
                    return 0;
               case 1: //同时计算加减速 
                    return getspeeddist_V1(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
                case 2:  //单加速 
                    return getspeeddist_V2(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
                case 3: //单减速
                    return getspeeddist_V3(flight_time, Distance, curr_time, fc_acc, fc_maxspeed, out vdist, out vspeed);
                default:
                    vspeed = 0;
                    vdist = 0;
                    return 0;
            }
        }

        }
    }