Servers by jDrones

Successfully using motion capture system as indoor GPS


(Chobitsfan) #1

Hi

I use Optitrack motion capture system to provide GPS navigation for indoor flight. I use Parrot Bebop and flash it to arducopter 3.5.5

flight video is here. I fly it in loiter mode using joystick through mission planner
desktop screen capture video show copter is in “3D fix”

I modify Optitrack NatNat SDK sample code SampleClientML.cs to transform local coordinate to WGS84 GPS coordinate.

P.S: I am using Motive 1.7. If you are using Motive 2.0+, it seems x axis is reversed

in processFrameData():

Console.WriteLine("\tRigidBody ({0}):", rb.Name);
Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);

double my_origin_lat = 24.773481;
double my_origin_lon = 121.0456978;
int epoch = 86400 * (10 * 365 + (1980 - 1969) / 4 + 1 + 6 - 2) - (18000 / 1000);
DateTime cur = DateTime.UtcNow;
TimeSpan ts = cur - new DateTime(1970, 1, 1);
int epoch_sec = (int)ts.TotalSeconds - epoch;

MAVLink.mavlink_gps_input_t gps_input = new MAVLink.mavlink_gps_input_t();
gps_input.ignore_flags = (ushort)MAVLink.GPS_INPUT_IGNORE_FLAGS.GPS_INPUT_IGNORE_FLAG_ALT | (ushort)MAVLink.GPS_INPUT_IGNORE_FLAGS.GPS_INPUT_IGNORE_FLAG_VDOP | (ushort)MAVLink.GPS_INPUT_IGNORE_FLAGS.GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY;
gps_input.fix_type = 3;
gps_input.gps_id = 0;
gps_input.hdop = 0.1f;
gps_input.speed_accuracy = 0.1f;
gps_input.horiz_accuracy = 0.1f;
gps_input.satellites_visible = 20;
gps_input.time_week = (ushort)(epoch_sec / 604800);
gps_input.time_week_ms = (uint)((epoch_sec % 604800) * 1000 + ts.Milliseconds);
gps_input.lat = (int)(((180.0 / (Math.PI * 6371008.8) * rbData.z) + my_origin_lat)*10000000);
gps_input.lon = (int)((my_origin_lon - (180.0 / (Math.PI * 6371008.8) * rbData.x))*10000000);
if (lastTime == DateTime.MaxValue)
{
gps_input.vn = 0;
gps_input.ve = 0;
gps_input.vd = 0;
lastTime = cur;
}
else
{
TimeSpan vts = cur - lastTime;
gps_input.vn = (rbData.z - lastPosZ) / vts.Milliseconds * 1000.0f;
gps_input.ve = (rbData.x - lastPosX) / vts.Milliseconds * -1000.0f;
gps_input.vd = (rbData.y - lastPosY) / vts.Milliseconds * -1000.0f;
lastTime = cur;
}
lastPosX = rbData.x;
lastPosY = rbData.y;
lastPosZ = rbData.z;
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.GPS_INPUT, gps_input);
mavSock.SendTo(pkt, mavEp);


Indoor flight with external navigation data
(Huibean Luo) #2

awesome,but the yaw data is abandon this way


(Chobitsfan) #3

the yaw data is abandon this way

Yes, however I do not know how to send it to ardupilot


(Huibean Luo) #4

Maybe mavlink vision interface would work?


(Chobitsfan) #5

You mean OPTICAL_FLOW? there is no orientation / yaw field there. Beside I found compass on the copter is enough for my application.


(Huibean Luo) #6

no,I mean computer vision


(Chobitsfan) #7

OK. I got it. I will try it. thank you :slight_smile:


(Tiziano Fiorenzani) #8

This is good. I manage to convert it in Python (remember to set GPS_TYPE2 = 14 (mavlink))

fix_type        = 5
sat_visible     = 20
epoch           = 315964782
cur             = datetime.datetime.utcnow()
ts              = cur - datetime.datetime(1970,1,1)
epoch_sec       = ts.total_seconds() - epoch
time_week       = epoch_sec / 604800.0
time_week_ms    = (epoch_sec % 604800) * 1000.0 + ts.microseconds*0.001

msg = vehicle.message_factory.gps_input_encode(
                     time_us,       # time_boot_ms (not used)
                     0,                 # gps_id
                     ignore_flags,      # gps_flag
                     int(time_week_ms),
                     int(time_week),
                     fix_type, 
                     int(lat*1e7), int(lon*1e7), alt_amsl,  
                     hdop, vdop,
                     vn, ve, vd,
                     spd_acc, hor_acc, vert_acc,
                     sat_visible)                # Satellites visible