For autonomous indoor flight, My first try is using optitrack motion capture system to simulate GPS coordinates to copter. The basic idea is to transform motion capture system local coordinate to GPS WGS84 coordinate and use GPS_TYPE=MAV(14) to feed GPS data to copter. The test result is here, it works ok. However there is a problem with this approach. It still need magnetometer/compass onboard copter to navigate. But compass performance could be very unstable due to many indoor interference, for example, air condition. and loudspeaker.
Thanks to @priseborough great job and @peterbarker great job, we can feed both position and orientation data to copter from external source. So that copter can fly autonomously without magnetometer. I use use mavlink message ATT_POS_MOCAP to send external nav data to copter through wifi (COMPASS_USE = COMPASS_USE2 = COMPASS_USE3 = 0, EK2_GPS_TYPE=3). The reason why I use ATT_POS_MOCAP instead of VISION_POSITION_ESTIMATE is that former accept quaternion and later accept euler angles. Because optitrack data API ouput quaternion. I do not need to convert it to euler angles.
I use optitrack motion capture system, iMotive software and parrot bebop running arducopter
First, I setup optitrack motion capture system. I install 5 markers on the copter. According to optitrack user manual, at least 4 markers is required for rigid body orientation. In the 1st time setup, I put copter in the optirack origin point, align copter to the optitrack x axis and copter nose to x positive (optitrack use right hand coordinate system).
Optitrack positive X will be north of our indoor environment (it need not be real north). Then I select all 5 markers to create a rigid body, this rigid body will be used to track position and orientation of copter.
system architecture is like: optitrack --> desktop PC (running iMotive and modified SampleClientML.exe ) - wifi -> arducopter
My code is modified from optitrack natnet sdk sample code SampleClientML.cs
Basically I only add following code to send position and orientation data to copter
MAVLink.mavlink_att_pos_mocap_t att_pos = new MAVLink.mavlink_att_pos_mocap_t();`
att_pos.time_usec = (ulong)((cur - my_start).TotalMilliseconds * 1000);`
att_pos.x = rbData.x; //north`
att_pos.y = rbData.z; //east`
att_pos.z = -rbData.y; //down`
att_pos.q = new float[4] { rbData.qw, rbData.qx, rbData.qz, -rbData.qy };`
pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.ATT_POS_MOCAP, att_pos);
mavSock.SendTo(pkt, drone.ep);
Note that ardupilot will drop consecutive att_pos_mocap message less than 70ms
void NavEKF2_core::writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms)
{
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
if ((timeStamp_ms - extNavMeasTime_ms) < 70) {
return;
} else {
extNavMeasTime_ms = timeStamp_ms;
}
Basically you do not need to send vision estimated messages faster than 15hz
You need to use SET_GPS_GLOBAL_ORIGIN to assign lat/lng of EKF origin for auto takeoff in guided mode (thanks @khancyr). It do not need to be the correct lat/lng, any valid lat/lng will work. This also makes copter able to fly auto mission. However, use lat/lng to assign waypoint may be a little difficult for indoor environment. I create a PR support MAV_FRAME_LOCAL_NED in MISSION_ITEM for auto mode. It allow me to use local coordinate to assign waypoints, which is more convenient. Please note that arducopter minimum RTL altitude is 2m. It may be a little high for indoor flight. You can change it in RTL_ALT_MIN in ArduCopter/config.h
Here is my test flight video. It is a parrot bebop running ardupilot master. It is auto takeoff and then switch to circle mode. The result is quite well and reliable (no magnetic interference from indoor environment)
this is my ardupilot parameters
mav.parm (20.9 KB)
