LatLong to XY

I have been using the UTM code from WHOI on an Uno to convert GPS latitude/longitude to UTM easting/northing. On an Uno, double and float are the same, so the easting and northing values are slightly different from the website. On a Due, double is 64 bits and the Arduino and website values are the same. latlon2xy is the doc with the Arduino code. Since I last looked at GPS, Mikal Hart has an updated GPS library that computes distanceBetween() and courseTo(). It might be worth pursuing.

Posted in Uncategorized | Leave a comment

Modest helicopter control progress

I bought an Arduino Due to use as the flight computer and a LIDAR-Lite from PulsedLight 3D to use as an altimeter.  Preliminary experiments with the LIDAR look promising; it’s certainly much better than the barometer/accelerometer complementary filter, but it’s a little pricey (for a hobbyist sensor) at $90.

Converting latitude and longitude to Cartesian coordinates is not trivial and I’ve been giving myself a crash course in geodesy and the Universal Transverse Mercator coordinate system. My controller uses Cartesian coordinates (makes sense since the NGLLC profile is up 50 m, over 100 m, and down 50 m), so I’ll use the UTM transformation to take latitude and longitude measurements from my GPS and turn them into (x,y).

Also, I’ve been (slowly) porting TJ Bordelon’s FreeSpace IMU algorithm into an Arduino library.

TODO (this week):

  1. Finish porting the FreeSpace IMU to Arduino.
  2. 3D print replacement landing gear for helicopter.
  3. 3D print special landing gear with mount for the Due, GPS, IMU, and LIDAR.

TODO (next week):

Helicopter system ID: Need to relate ESC command signal to thrust and servo command signal to “gimbal” angle.

Posted in Uncategorized | Leave a comment

Helicopter, IMUs & State Estimation


A 4 channel, fixed-pitch, RC helicopter is a good analogy for my VTVL rocket. Both vehicles control altitude by changing the magnitude of the thrust vector and both vehicles control attitude by changing the direction of the thrust vector. On the rocket, the magnitude of the thrust vector is controlled by opening and closing the main propellant valves. On a fixed-pitch helicopter, an ESC controls how fast the main rotor spins. On the rocket, the direction of the thrust vector is controlled by gimbaling the motor via linear actuators. On the helicopter, servos connected to the swashplate cyclically pitch the main rotor blades. Paul Breed used a T-Rex 600 to test his GNC software and extensively documented the process on his blog.

I purchased a Blade 200 SR X, extra blades, extra battery, and a simulator; my plan is to create control hardware and software for the helicopter that can be ported to the rocket. Most of the past week has just been learning to fly the simulator, but I did put some effort into hardware. A nice thing about the Blade 200 SR X is it uses conventional hobby servos and ESCs (many of the small helis have dedicated boards which serve as receiver, ESC, and servo) so it’s easy to control with a microcontroller. However, I didn’t know that ESCs have to be armed before you can send them a PWM control signal. On my helicopter, firmware in the receiver sends the ESC the arm command a few seconds after it powers on. This is what the receiver sends the ESC to arm:


The frequency is 344 Hz and the duty cycle is 34.4%. I created this pulse train using an Uno and the digitalWrite() and delayMicroseconds() functions and successfully armed the ESC.

IMUs and State Observer

The VTVL rocket has 12 states (6DOFs plus the time derivative of each DOF) and I need an estimate or reliable measurement of each state in order to control the rocket. Plan A is to obtain yaw, pitch, and roll from an IMU, position and altitude from GPS, and the rest of the states from a state observer. Masten and Armadillo flew KVH and Crossbow IMUs built around fiber optic gyroscopes. These units retail for more than $10,000 and are prohibitively expensive. Several companies (VectorNav, Xsens, LORD MicroStrain) sell IMUs based on MEMS sensors, which fuse magnetometer, accelerometer, and gyroscope readings together via a Kalman filter to produce reliable estimates of yaw, pitch, and roll. These units retail for at least $1,000, which is still a little expensive for my tastes.

Last winter I started experimenting with Fabio Varesano’s FreeIMU. I built a few of them (my first time with SMD components) and they worked pretty well, but I couldn’t get rid of the yaw drift. The FreeIMU has good hardware, so I’m going to try to write my own sensor fusion code based on TJ Bordelon’s FreeSpace algorithm.




TJ uses a three-axis accelerometer, a three-axis magnetometer, and a three-axis gyroscope, and fuses them together in three steps.

  1. Integrate the gyro output to obtain a quaternion which rotates you from the body-fixed frame to the earth-fixed frame. This quaternion is incorrect because of the gyroscope bias. The calculated attitude will drift over time.
  2. Measure the acceleration in the body-fixed frame and rotate it to the world-fixed frame using the quaternion from Step 1. The answer should be [0 0 1] g. Use the difference between [0 0 1] and the calculated world-fixed acceleration to correct your quaternion. This step compensates for errors in pitch and roll.
  3. Measure magnetic field in the body-fixed frame and rotate it to the world-fixed frame using the quaternion from Step 2. The compass should be pointing North, [1 0 0]. Use the difference between [1 0 0] and the the calculated world-fixed magnetic field to correct the quaternion. This step compensates for error in yaw (or heading).

The amount of money Masten and Armadillo spent on their IMUs speaks to their importance. But my goal is a 30 second flight, not a 180 second NGLLC Level 2 flight, and I’m cautiously optimistic that 30 seconds is short enough to make my homemade MEMS solution viable. If hobby-grade sensors don’t work, Plan B is the VectorNav VN-100 Rugged DevKit for $1000.

Posted in Uncategorized | Leave a comment


I made a model for a notional VTVL vehicle and tried to build control laws to control altitude, lateral position, and attitude angle for a 30 second flight. The control strategy I’m using is:

• During ascent, use a PD controller to hold altitude.
• During descent, use a proportional controller to track and hold descent velocity.
• Full state feedback to keep attitude angle and lateral position at zero.
• Turn off motor when switches on lander feet detect contact with the ground.

The free body diagram of the planar vehicle looks like this:


The vehicle has 3 degrees of freedom: altitude y, lateral position x, and attitude angle θ. The only input is the thrust vector from the motor which has magnitude T and a direction controlled by the gimbal angle φ. The mass of the vehicle decreases as a function of time; the mass flow rate varies according to the required thrust and an assumed motor specific impulse. The equations of motion (EOMs) are coupled and nonlinear.


I used the linearized EOMs for the controller design.


The linearized EOM for altitude (Equation 5) describes a SISO system where thrust is the only input and altitude is the only output. Frequency domain, PID, or root locus techniques would work controlling for this system. After some iteration, I achieved good results using a PD controller to track an altitude setpoint during ascent and hover and a proportional controller to track a descent velocity. For both cases, I feed forward the instantaneous weight of the vehicle and add it to the control effort calculated by the PD and P controller. Some combination of barometric pressure, acceleration, and GPS should produce a decent altitude measurement.

Equations 4 and 6 describe a SIMO system where lateral position and attitude angle are both controlled by gimbal angle. I used full state feedback to regulate lateral position and attitude angle to zero. In reality, I probably won’t have access to all the states and will need to design a state estimator/observer. My plan is to measure position with GPS and attitude angle with a MEMS IMU (fusing accelerometer, gyro, and magnetometer measurements) to produce yaw, pitch, and roll. I’ll have the position and attitude angle states, the velocities will come from the estimator.

I simulated a climb to 4 meters, 15 second hover, and descent at 0.4 m/s by numerically integrating Equations 1 through 3. At t = 10 seconds, the vehicle is struck by a disturbance force and the controller stabilizes the vehicle and returns it to its original position. Here’s the MATLAB simulation code (hovercontrol), saved in a .doc – I’ll upload .m files if I can figure it out.



Posted in Uncategorized | Leave a comment