Successfully using motion capture system as indoor GPS

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);

2 Likes

awesome,but the yaw data is abandon this way

the yaw data is abandon this way

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

Maybe mavlink vision interface would work?

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

no,I mean computer vision

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

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
3 Likes

Hi tiffo,

I have always find your online tutorial really helpful. It will be really great that you can do a online tutorial in demonstrating the setup procedure for indoor flight too. Cheers

Me and a friend at the Uni have been working on that

Great work! I’ve been trying to copy this, but I can’t get it to work. I am completely new to drones / ardupilot. We have a Optitrack system and are running Motive 2.1.1. I’ve tried for some days now, but I’m pretty overwhelmed with it all. Would you be open to helping us getting our drone / Optitrack system working? I would be open to paying you for your efforts.
Thanks John

hi @jbausano please refer http://ardupilot.org/copter/docs/common-optitrack.html

@chobitsfan Yes, I’ve seen that post, that’s why we bought the drone we did, we’re trying to mimic that good work you did.

When I generate the c# headers I’m not seeing the same files you listed in that post. Actually c# generation fails, you have to edit one of that python scripts. I need help with the creating mavlink c#, and exactly how to incorporate that into natnet code.

Again, I’m willing to pay for your or anyone’s efforts that helps us get this working soon. Thanks again, John.

When I generate the c# headers I’m not seeing the same files you listed in that post.

I am very sorry. I made a big mistake in the post. Mavlink.cs, MavlinkMessage.cs, etc are actually coming from mission planner. You can find them in https://github.com/ArduPilot/MissionPlanner/tree/master/ExtLibs/Mavlink
just copy all .cs file in that folder , you do not need to generate them yourself.
I will fix the doc, thank you

Great thanks, I’ll try that!

@chobitsfan, thanks for the help so far. I’ve got natnet sending position data through mavlink now. i’ve created a little python script to print the yaw, pitch and roll real time, and I can see the numbers changing as they should when I move the drone.

I’m now stuck on getting the drone to actually fly. What, how do I use GPS_GLOBAL_ORIGIN? Would you have a simple python script that launches the drone, moves to 1 waypoint and lands?

Thanks

hi @jbausano
you can use dronekit

I am getting an error “mavlinkparse is not used in current context” how do i solve it.

this should be fixed in https://github.com/ArduPilot/ardupilot_wiki/pull/2108