Ardupilot基于UWB的定位飞行测试

    拖了许久的测试视频，终于要发布了，上个版本的代码有点问题，没有考虑清楚UWB和NED坐标系的关系，导致后面飞行故障不断，这款UWB的坐标系为ENU坐标系，飞机飞行的坐标系是NED坐标，那就需要将UWB的位置数据X和Y数值颠倒，Z数值取负值，这样就把UWB的数据转换到NED的坐标系下；

float vehicle_x = (float)Byte32(sint32 ,linebuf[6],linebuf[5],linebuf[4], 0) / 256000.0f;
    float vehicle_y = (float)Byte32(sint32 ,linebuf[9],linebuf[8],linebuf[7], 0) / 256000.0f;
    float vehicle_z = (float)Byte32(sint32 ,linebuf[12],linebuf[11],linebuf[10], 0) / 256000.0f;

    //hal.uartE->printf("vehicle_x=%f\r\n",vehicle_x);
    //hal.uartE->printf("vehicle_y=%f\r\n",vehicle_y);
   
    Vector3f veh_pos(Vector3f(vehicle_y, vehicle_x, -vehicle_z));
          
        if (veh_pos.length() <= AP_BEACON_DISTANCE_MAX) 
        {
                set_vehicle_position(veh_pos, 0.02f);
                parsed = true;

                if (parsed)
                {
                    last_update_ms = AP_HAL::millis();
                }
    
        }

解析数据可以通过串口打印出来；
另一组数据，UWB的坐标配置，同样需要进行坐标系转换；

Vector3f beacon_pos(beacon_y[i] / 1000.0f, beacon_x[i] / 1000.0f, -beacon_z[i] / 1000.0f);
               if (beacon_pos.length() <= AP_BEACON_DISTANCE_MAX) 
               {
                
                   set_beacon_position(beacon_id[i], beacon_pos);

第三组数据的发送，飞机距离每个基站的距离信息，如果UWB的原始数据有这项信息，那就可以解析出来距离信息发送出去，如果没有，只需要将飞机在基站坐标系的位置信息与基站的坐标信息做差取向量，求模即可。

float beacon_distance = (beacon_pos - veh_pos).length();
                   if (beacon_distance <= AP_BEACON_DISTANCE_MAX)
                   {
                        set_beacon_distance(beacon_id[i], beacon_distance);
                 //hal.uartE->printf("beacon_distance=%f\r\n",beacon_distance);          
                   }

地面测试阶段，链接MP地面站，这里MP有个隐藏功能，Ctrl+F，观测mavlink inspector 这里有EKF融合后的位置数据local position，观察XY的位置跳动，如果静止情况下，波动范围小，再测量一下动态性能，位置如果可以稳得住，就可以实飞测试了。