Palo Alto Foothills Park

We went to Palo Alto Foothills Park yesterday afternoon. We were actually trying to go to the Pearson-Arastradero preserve, but ended up here instead. It was fortunate SC still has a Palo Alto address on her driver's license, otherwise they wouldn't have let us in.

There was a pond full of fuzzy stem plants (and 3 canoefuls of Boy Scouts). And ducks.

SC thought I had a particularly awkward shooting pose. I'm planning on sending the film to Snapfish for processing, so it'll probably be a few weeks before we find out how those pictures turn out.

I thought this hill was pretty interesting - trees and brush on the left, yellowed grass on the right. Is it the sun? This photo was also taken in HDR mode on the iPhone - the colors became muted and there's a halo around the hill, but neither the bright sky nor the dark leaves got washed out.

It was pretty awesome how colorful these were. 

I used Windows Live Photo Gallery (what a mouthful!) to stitch together four photos for this panoramic. The software's not bad; I think the problem here was the auto-exposure settings on the camera.

And as we were heading out, we saw this deer carcass in a clearing by the road. I hope it was mountain lions.

Rasbora update

Saw some interesting behavior in two of our rasboras today:

That's the big fish (two-stripe fish), chasing around one of the smaller fish, possibly trying to mate with her and probably doing it wrong. I took this video on my iPhone 4 - the camera on it is pretty slick.

The pumpkin pie succumbs to mold!

After eating less than half the pie over the last few days, it has succumbed to mold, which is rather disappointing. Next time, we'll refrigerate. Or maybe get some calcium propionate.

Pumpkin pie

After our failed trip to the Half Moon Bay pumpkin festival, we bought a pumpkin at Safeway with the intention of making pie. Turns out those Jack-o-lantern pumpkins are different from those typically used for making pie (i.e., larger, more watery, less sweet, and grainier).

Still, we went ahead and made pie following this recipe. We made a graham cracker crust and used fresh ginger and only 1 can of evaporated milk (I didn't realize that it's so different from condensed milk!), but otherwise followed it exactly. If you're wondering what the Cateye bike light is doing in the photo below, it's how we look into our oven, for there is no window nor light in there.

It turned out quite nice! We drained a lot of liquid out of the pumpkin puree just by letting it sit on a colander for awhile and stirring occasionally. It was still quite soupy after mixing in the evaporated milk, but it firmed up in the oven. It's quite spicy, but I guess that is what one expects from pumpkin pie. Compared to pumpkin pie we've gotten at stores, it's fluffier and less sweet, both of which are positives in my mind.

Heat is on!

Housing turned on the heat today. Now we can't figure out how to turn it off! The thermostat in the kitchen seems to work to regulate the kitchen/living room heater, but the bedroom heater seems to be on constitutively. I did find this handy link for instructions on how to adjust the heater settings, though! Having the heat on has caused a drastic increase in the humidifier output rate, which was expected. Now we'll really see if it's effective.

Speaking of heating, I'm pretty proud that our heating comes from steam generated at a cogen plant on campus (a combined-cycle one at that). Whenever you run a steam turbine, you will need to condense the steam back into water. Cogen means using the district's heating needs as part of that condenser. The primary purpose is to generate steam for heating, but running the steam through a turbine (and while you're at it, using a gas turbine to generate the hot air for boiling the steam) allows you to opportunistically generate electricity at high efficiency. The cogen plant also runs steam-powered chillers to provide process cooling water.

There is much talk, though, of switching to a hot-water district heating system using electric-powered regeneration chillers (heat pumps to simultaneously chill the chilled water and heat the hot water) as part of Stanford's green energy initiative. It would reduce water use from steam leaks and evaporative cooling towers. Whether or not it reduces greenhouse gas emissions will depend on where we buy our electricity from, and the projected costs will depend on the volatility of natural gas prices, so it's hard to say definitely whether it's a good idea, but not taking advantage of this cogen opportunity seems like a waste.

Helicopter SAS: Overview

This post will be an overview of my helicopter Stability Augmentation System (SAS) project.

The Story

This last spring, I decided to build a stability augmentation system (SAS) for an R/C helicopter, with the goal of giving my dad an easy-to-fly R/C helicopter that still had the coolness factor of a single-rotor design.

I spent the next two months designing, building, and debugging the SAS. I don't think I could have flown the helicopter without the SAS active, but then again I haven't tried.

I did crash it a few times, though, and the unbalanced blades and bent shafts definitely degraded performance. I also broke the landing gear and cyanoacrylated it back together. During my demo for my dad (my family came to town for graduation), I touched down too hard and one side of the landing gear broke, causing the helicopter to flop over onto that side.

My dad took it home; I think he's repaired the crash damage but I don't think he's flown it. So, I haven't been quite successful in that regard. Maybe next time...

The Parts

The SAS is implemented as this foil-shielded box that sits where the original heading-hold gyro used to be. Inside the box are sensors to measure the helicopter's motion and orientation. The lid of the box is the control board that uses those measurements to send stabilizing control signals to the helicopter servos.

Barely visible in this photo is the wireless module taped to the rear strut of the right (starboard?) landing skid. I used the telemetry data extensively during testing and debugging.

Suggestions for Next Time

• Velocity feedback (optical flow)
• Heading measurement (magnetometer w/ offset for body-fixed fields)
• Transformation of cyclic commands from pilot axes to body-fixed axes
• Different mounting location/method to minimize vibration at accelerometers
• Don't crash it as much

No pumpkins for us

We were planning to go to the Half Moon Bay pumpkin festival today, but the traffic on highway 92 was ludicrous. The iPhone confirmed that it was stop-and-go all the way to Half Moon Bay. We sat on the onramp for 30 minutes and took the first opportunity we had to make an illegal U-turn and come back home. I'm so glad I don't have to sit in traffic as part of my usual commute...

Tennis racquets

SC and I decided that we should get into tennis because we have a free court right next to our apartment.

Ever the cheapskates, we decided to buy the cheapest ones we could find on eBay. This is what happened:

I wish I had a tennis ball to show the scale, but the point is that they're quite small. The handle is about the right size, so I guess back in the day you had to be more accurate with your swings...

Making Solidworks references more resilient

Making references to other parts is often inevitable (or else the alternative is much more work). They often cause problems, though, and after some trial and error this is what I've learned:

1. Consider using "Intersection curve" on a solid body rather than "convert entities" with a face or edge. It breaks less frequently because Solidworks can still find where the solid body intersects the sketch plane, whereas some common operations will destroy the face/edge that "convert entities" used to refer to.
2. Avoid splines. Also, the intersection of two curved surfaces or an offset of an ellipse will generally be a spline.
3. You can use "offset entities" on an entire sketch (rather than selecting sketch elements) if it is a single loop. This way, the offset still works even if you add/delete sketch elements.
4. Tongue/groove features on mating edges seem especially prone to breaking. So just don't bother with them (suppress them while you change upstream features) until you're ready to send it out to SLA or whatever.
5. If you define features for a part in the context of an assembly, make sure you that assembly always accompanies that part. As a corollary, if you decide to rename the assembly, do this in Solidworks Explorer, not "Save As...". Otherwise it's very easy for features to become out of context (looks like "-> ?").
6. Try to plan out part dependencies. If you can create one set of independent parts (that define mating surfaces, critical geometries, etc.) and one set of parts that depend only on those independent parts, you can reduce rebuild times and reduce the likelihood of crashing.
7. Fillets on complex surfaces break all the time. Don't reference fillet edges.
8. If you need to make a copy of a surface, "Knit surfaces" fixes small gaps but "Offset surfaces" does not.

Photo develop + scan houses

After doing some digging, I found some cheap places that develop and scan photos.

Developing

From what I understand, the main source of variability between professional developers is not the quality of the developing (that's just basic process control) but their care in handling your negatives.

The cheapest seems to be York Photo. Their prices come out to 0.18 (24 exp) or 0.15 (36 exp) per photo including 4x6 prints and 1612x1024 downloadable jpeg's.

Snapfish also does photo developing. They're owned by HP now, but they used to be owned by the same company that owns York Photo, so it's likely they use the same photo processor (in Bangalore, I think). Their prices are 0.21 (24 exp) or 0.14 (36 exp) per photo including 4x6 prints and online photos, plus 0.06 per photo to download 1500x1000 jpeg's.

I'll try out both of them and see how the scans come out. It'll have to be on different rolls of film, but at least I can look for egregious differences in scan quality.

I also looked at Mpix (couldn't get a straightforward price, but at least 0.19 per photo) and the labs from Ken Rockwell's blog (much more expensive, and not local for me). Costco sounds good, but I don't have a card.

Locally, Keeble & Shuchat does same-day turnaround. Develop only is 0.21 (24 exp) or 0.14 (36 exp) per photo, but adding a 1536x1024 scan brings it up to 0.63 (24 exp) or 0.42 (36 exp) per photo. Prints are extra, and higher resolution (3000x2000) adds another 0.23 (24 exp) or 0.15 (36 exp) per photo.

For specialty work, they use Swan Photo Labs, who goes 0.56 (24 exp) or 0.38 (36 exp) per photo at 1536x1024, or 0.79 (24 exp) or 0.53 (36 exp) for 3088x2038. They do throw in free replacement film, though, for whatever that's worth. Oh, wait: I know how much that's worth: about $2/roll (0.06 per photo).

Scanning

I'm not satisfied with these 1.6 megapixel scan resolutions. It's a good preview, enough for a blog post, but not even for a desktop background. And for a 300 dpi print that's little over 3x5. Furthermore, digital photo habits have also left me reliant on judicious cropping, so the "good" portion has much smaller dimensions.

So a specialty scanning house may be called for. Since there's more technique involved, I'd willing to accept that you get what you pay for.

ScanCafe seems like the cheap option that at least makes a lot of noise about having high quality. They have some kind of sale right now (0.29 per scan, down from 0.35). Since I don't plan on cutting apart the negatives, this is worst-case 0.92 per photo (1 in 4 worth keeping, 80% minimum on "pay only for those you want to keep"). These are 3000 dpi scans (3900x2625); upgrading to 4000 dpi (5200x3500) adds 0.09 per scan.

Larsen Digital seems like a pretty reputable place. They charge 0.35 per scan (worst-case 1.40 per good photo) at 4000 dpi, but have a $40 minimum and at least $18 shipping on top of that.

Results forthcoming!

I've signed up with York Photo and Snapfish, so they should be sending me welcome packages with pre-paid mailers. I'm also following up with York on a pricing discrepancy (the prices I listed are the prices on their "help - pricing" page, which are different from those on the "print out a mailer" page). I'm also waiting on some film I ordered from Adorama (Fujicolor Superia 400, Fujicolor 100, and Kodak Ektar 100).

I'm really excited though! If the film arrives before this weekend, I can take pictures of pumpkins!

Helicopter SAS: Control module

This post will describe the main control module of my helicopter stability augmentation system (SAS).

Summary

This is the main control board of the SAS. It sits between the helicopter radio receiver and the servos, interpreting the servo commands from the receiver and adding a stabilizing input before relaying these commands to the servos. It operates using feedback based on helicopter orientation information from the AHRS module.

Hardware

The module consists of an Arduino Pro Mini 328 (3.3V, 8MHz) mounted on a board I designed (fabbed by AP Circuits). The Arduino is connected as follows:

Arduino pin	ATMega328 pin	Connected to
2	PD2	Servo in (left cyclic)
3	PD3	Servo in (front cyclic)
4	PD4	N/C
5	PD5	Servo in (tail)
6	PD6	Servo in (gyro control)
7	PD7	Servo in (right cyclic)
8	PB0 (ICP1)	Accessory header
9	PB1 (OC1A)	Shift register Clock
10	PB2 (SS)	White wire to AHRS
11	PB3 (MOSI)	AHRS header
12	PB4 (MISO)	AHRS header
13	PB5 (SCK)	AHRS header
A0	PC0	Shift register Data
A1	PC1	Red LED
A2	PC2	Shift register Clear
A3	PC3	Green LED
A4	PC4 (SDA)	Accessory header
A5	PC5 (SCL)	Accessory header
VCC	VCC, AVCC	N/C
GND	GND, AGND	Ground
RAW	N/A	+5V
DTR	PC6 (RESET)	Programming header
RXI	PD0 (RXD)	Programming header
TXO	PD1(TXD)	Programming header

Since the helicopter runs at 5V and the SAS at 3.3V, all inputs are buffered through a 74LS07D 3.3V hex buffer with 5V-tolerant inputs, and the outputs are driven by a 74HCT164D 5V 8-bit shift register with TTL-compatible inputs.

The only other components on the board are a 3.3V linear voltage regulator, some bypass capacitors, and two LEDs.

Communications

Under normal operation, the SAS main board communicates with the Attitude and Heading Reference System (AHRS), helicopter radio receiver, and helicopter servos. Furthermore, it has connections for programming/debugging and a few extra pins that are not currently used.

AHRS

Since the AHRS module's SPI SS pin is not externally accessible, this board was originally designed not to connect the SS line. However, this ultimately proved infeasible, so there was some scraping of traces and now there is a white wire that connects from an AHRS digital output (the AHRS is the SPI master) to the Arduino's SS pin.

The other SPI lines (SCK, MISO, MOSI) are connected normally. This board also supplies the AHRS module with 5V power (AHRS module has on-board 3.3V reg).

Helicopter radio receiver

The helicopter radio has six output channels: 3 for cyclic servos, 1 for the tail servo, 1 for the motor speed controller, and 1 for gyro gain control. As a failsafe measure, the motor speed control channel is connected directly to the receiver, bypassing the SAS altogether. The other 5 channels are connected to Arduino digital inputs.

These servo signals are encoded using pulse width modulation (PWM). The range 1 to 2 ms nominally corresponds to the servo limits of travel. The radio receiver generates a pulse on each channel sequentially and refreshes at approximately 50 Hz.

The radio receiver also provides this board with 5V power.

Helicopter servos

The SAS communicates with the helicopter servos using the same PWM scheme. Since only one pulse will be active at a time and the pulses can be sent sequentially, the 74HCT164 shift register (no latch) is well suited for this application. This allows us to control up to 8 servo channels using 3 digital outputs with no loss of functionality.

Since servos can draw a lot of power, the servo power supply lines are not routed through the SAS module.

Programming/Debugging

In order to facilitate programming and debugging, the main board also brings out the UART's RX and TX lines. The RESET pin is also connected to the serial port DTR line via a high-pass RC circuit (on the Arduino). This is used during programming to activate the Arduino bootloader before transferring code.

The programming/debugging header also provides 3.3V power for a wireless module.

Other connections

The SAS main board also brings out the Arduino's ICP1 (input capture on timer 1) pin. This could be used with a main shaft encoder to measure rotor speed, but that hardware was never implemented.

The I2C bus (SDA and SCL) is also available in the accessory header, but so far there is nothing to connect to it.

Control law

A quick note on terminology: the "control law" is the algorithm used to calculate the "control output" (signals sent to helicopter servos) based on the "command input" (signals from helicopter radio receiver, which originated from the human pilot via radio transmitter) and feedback from the AHRS. The servos change the angles of the helicopter blades, generating aerodynamic forces and moments that act on the helicopter. The resulting rotations and accelerations are measured by the AHRS.

The goal of the SAS is to produce control outputs to return the helicopter to upright (pitch and roll angles = 0) and not spinning (yaw rate = 0). If the pilot sends a command input, then the SAS should add that to the control output, and it should not be too obtrusive about it.

In order to keep things simple, I attempt to control each axis independently of the others.

Axis	Actuator	Control law
Pitch (tilt up/down)	Cyclic (long.)	u = c - KPθ - KDω
Roll (tilt left/right)	Cyclic (lat.)	u = c - KPθ - KDω
Yaw (turn left/right)	Tail rotor	u = ( KP + KI / (τIs+1) + KDs/(τDs + 1) )(c - ω)
Lift	Collective	u = c (no feedback)
Rotor speed	Throttle	u = c (physically bypasses SAS)

The control laws in the table above should be read as schematic representations of the type of control law used, not the actual formulae used to calculate control output. u is the control output, c is the command input, θ is the orientation angle, ω is the rotation rate, s is the Laplace...thing (variable? argument?), K's are gains, and τ's are dynamic compensator parameters.

System identification

In this case, the complexity of the control law was limited by the quality of the system identification. There exists significant coupling between axes and all sorts of interesting modes involving combinations of rotational and translational motion. The literature describes what coefficients would go into a state-space model to capture these dynamics, but I simply didn't perform the measurements necessary to measure these effects.

In the literature, system identification is performed by getting a competent pilot to input a frequency sweep while keeping the helicopter under control in near-hover conditions. This was a Catch-22 for me; if I could pilot the helicopter well enough to do this, I wouldn't need to design a control system!

For me, system identification consisted of programming the SAS to output a frequency sweep on each actuator sequentially, then holding the helicopter by the skids at arm's length and praying for my safety as whirling blades of death heaved to and fro with surprising unpredictability. There is a tradeoff here between holding the helicopter lightly so as not to influence the measurement and holding the helicopter firmly so that it doesn't whack you in the face during the low-frequency portion of the sweep. And now that I've seen what a crash can do to a 3mm steel rod, I'm not sure I'd use this method again.

After risking life and limb to carry out these experiments, I transformed the telemetry results into the frequency domain and estimated two first-order models for pitch and roll and a second-order model for yaw.

Software functional description

Much of the coding frustration associated with the SAS involved dealing with non-negligible communication and computation latencies. Rather than step through the various code modules, I will describe some of the more interesting interactions between modules.

Maintaining timing accuracy with multiple interrupts

Receiving data from two asynchronous sources (AHRS and radio receiver) proved to be a challenge. The Arduino needs to accurately measure the servo command input pulse widths. Since there are not enough input capture channels, I implemented this measurement using the pin change interrupt, which can be configured to be triggered by an edge on any of the servo input pins.

However, the Arduino also needs an interrupt service routine for communications with the AHRS (as the SPI slave, it needs to respond within 32 μs of a received byte). Since this may conflict with the input servo pulse timing, this ISR needs to be kept as short as possible. By hand-optimizing of the ISR prologue, I trimmed it down to 46 clocks (5.8 μs).

Servo control output is also interrupt-driven: we shift a single high bit into the shift register and then use the timer module (output compare pin A) to produce correctly-timed rising edges on the clock line, thus sending sequential pulses to the servos. Since servo output never overlaps with servo input, this interrupt does not conflict with input pulse timing.

Interrupts, queues, and normal-priority routines

In order to keep the interrupt service routines as short as possible, they interact only with queues and perform the minimum amount of processing required. Further processing of the data is performed in a normal-priority routine called from the main loop.

Function	Interrupt source	ISR	Normal-priority routine
Servo input	Pin change	Push current timer value and pin state onto queue.	Pop pin states and timer values off queue; calculate pulse duration for the associated channel(s). Update global servo input array and flag the channel(s) as having been updated.
AHRS input	SPI serial transfer complete	Push received byte onto queue.	Check if a full 38-byte frame has been received. If so, pop bytes off queue and re-cast into correct data type. If checksum is correct, update global helicopter orientation arrays.
Servo output	Timer 1 output compare match	If queue is empty, disable interrupt. Otherwise, pop next pulse duration off queue and add to output compare register.	Set data pin high to shift out a single high bit. Enqueue desired pulse durations, set the output compare register to 50 μs from now, and enable interrupts.

Sending telemetry over the serial port also involves a queue structure. However, since it is an auxiliary task, it does not get the privilege of its own interrupt. The dequeue function, which checks if the UART is ready and pops the next character into the transmit register, is called from the main loop. This is less efficient, but prevents further interference with the critical servo input timing.

Main loop description

The main loop performs the following tasks:

1. Call the AHRS input routine. If it indicates that a full data frame has been received:
1. Calculate the feedback portion of the control law using the newly-provided orientation and rotation rate data.
2. Call the servo input routine. If it indicates that a full servo update has occurred:
1. Calculate the feedforward portion of the control law using the new servo commands.
2. Check the "gyro control" servo channel:
1. If < 0, this indicates the "Gyro enable" switch on the radio transmitter is in the "1" position. Sum the feedforward and feedback portions of the control law to get the SAS control output.
2. If > 0, this indicates the "Gyro enable" switch is in the "0" position. Set the control output equal to the command input (i.e., don't add any additional control).
3. Call the servo output function to begin sending pulses.
3. Check if 20 ms has passed since the last telemetry update. If so:
1. Begin formatting and queuing telemetry data to be sent. This actually takes a significant amount of time. In order to give the other routines a chance to update, this is broken into 4 tasks that will be performed over the next 4 main loop iterations.
2. Update LEDs based on whether the AHRS (green LED) and servo input (red LED) have updated since the last telemetry cycle.
4. Call the serial port dequeue function to send the next byte of telemetry data, if the UART is ready.

This order of event handers ensures the control algorithm is always using the most recent information. Breaking up the control law calculations reduces the average latency between receiving a servo command input and issuing a servo control output.

Hummingbird feeder

One of our first purchases at our new apartment was a hummingbird feeder to put on our balcony.

Here you can see the piece of aluminum I bent and zip-tied onto the railing to mount the feeder.

I took these photos using a 35mm SLR with an 80mm zoom lens. I'd actually gotten the camera about 10 years ago, and it's been sitting in a closet at my parents' house ever since the digital revolution. I'm pretty excited about taking some more photos with it. I'll try a higher speed film (this was 100 ISO) for future hummingbird shots. I also need to source a cheap photo developer/scanner; these were done at Keeble & Shuchat, who charged $4 for developing plus $10 for the image CD, which came as 1536 x 1024 jpegs.

We fill the feeder with a 1:4 mix of white cane sugar and tap water. We have to change the water pretty frequently (a few days at most) or else it starts to smell like yeast, suggesting fermentation. It doesn't get drunk very quickly, so I only refill it with 3/4 cup at a time - the picture above is the "full" state.

This guy is the reason why the feeder goes so slowly. He spends most of the day hanging out on a nearby branch, ready to dive-bomb any bird who approaches the feeder. All the photos above are probably of this one bird. Sometimes he will leave his post (hopefully he is off eating bugs like he ought to!) and other hummingbirds will come and drink furtively from his precious, but he always returns soon to chase off the trespassers. He's been very easy to anthropomorphize in this regard.

Helicopter SAS: AHRS module

This post will describe the AHRS (Attitude and Heading Reference System) module of my helicopter stability augmentation system.

Summary

The AHRS provides the sensing component of the stability augmentation system (SAS). It estimates the helicopter's orientation using measurements from the onboard accelerometer and gyro. Ever 20 ms, it reports an updated estimate to the SAS main board over an SPI serial interface.

Hardware

This section describes the hardware and communications connectivity of the AHRS module.

The AHRS uses an unmodified SparkFun 9 degree of freedom Razor IMU (rev 14, with the 8Mhz ATmega328P). The board comprises:

1. ATmega328P 8-bit microcontroller with 8 MHz clock
2. 3-axis accelerometer
3. 2-axis (pitch/roll) and 1-axis (yaw) gyros, with 48 Hz lowpass filter
4. 3-axis magnetometer
5. 6-pin header for Sparkfun's FTDI USB-UART breakout board
6. 6-pin header for ICSP
7. 2-pin header for unregulated power supply

The microcontroller interfaces with the accelerometer and magnetometer via I²C and with the gyros via its ADC channels. The UART header brings out the RX and TX pins. The ICSP header brings out the the SPI MISO, MOSI, and clock pins, but unfortunately not the slave select pin, so we can only operate the SPI module in master mode.

Under normal operation, the AHRS communicates with the SAS main board over SPI. The UART module is disabled and the RX pin used as a digital output to drive the SAS main board's SPI slave select pin. That is the purpose of the white wire in the photo at the top of this post. 

The AHRS board is mounted inside a polycarbonate enclosure wrapped in aluminum foil. When the main board is installed on top of the box, its ground plane makes contact with the foil, creating a shielded environment for the sensors.

Estimator Algorithm

This section describes the quaternion-based kinematic estimator implemented as a first-order linear complementary filter between gyro measurement of rotation rate and accelerometer measurement of gravity vector.

The purpose of the AHRS is to provide estimates of rotation rate, orientation, and translational acceleration to the SAS. The gyros directly measure rotation rate, but the accelerometers measure a combination of gravitational and translational acceleration, so we need an estimator to separate the two. For now, we are not using the magnetometer because the local magnetic fields from the helicopter's motors disrupt the measurements.

First, the sensor measurements are filtered with an 8 Hz (50 rad/s) first-order digital lowpass filter. This helps mitigate the noise (mechanical vibration) produced by the main rotor. Combined with oversampling (estimator loop runs at 50 Hz, but gyros are sampled at 500 Hz and accelerometer at 200 Hz), this also increases measurement resolution, but that's a moot point given the low signal-to-noise ratio in flight conditions.

Unlike a dynamic estimator, which uses the dynamic equations of motion (F = ma), a kinematic estimator uses only kinematic equations (v = dx/dt, a = dv/dt). In this case, we estimate orientation (rotation angles) from gyro measurements (rotation rates) and gravity vector measurements (rotation angle error). Implemented in discrete time, the estimator looks like:

1. At time t₁, we have an estimated orientation q₁, a quaternion representing the rotation of the helicopter from its reference position to its current orientation. Therefore q₁ also represents the transformation from body-fixed axes to inertial axes, and q₁^-1 the transformation from inertial to body-fixed.
2. We can predict that the gravity vector, measured in body-fixed axes, will be g_pred = q₁^-1g₀q₁, where g₀ is the gravity vector in inertial axes, i.e. [0;  0;  1].
3. The actual accelerometer measurement a_meas gives us two things:
1. First, the translational acceleration is simply the residual acceleration: a_trans = a_meas - g_pred
2. Second, assuming a_trans averages out to zero, our measured gravity vector is simply our measured acceleration, normalized to one gee: g_meas = a_meas/||a_meas||
4. Thus, step 3.2 gives us one estimate of orientation error: for small angles, Δθ_grav = g_meas × g_pred, where Δθ_grav is the additional rotation we need to apply to q₁ in order to bring the predicted gravity vector in line with the measured gravity vector.
5. However, our rotation rate measurements give us another estimate of orientation error: Δθ_gyro = ωΔt where ω is the gyro rotation rate measurement.
6. This is where the complementary filter steps in. For a crossover frequency f_C << 1/Δt, the update law Δθ = Δθ_gyro + (2πf_CΔt)Δθ_grav is equivalent to low-passing Δθ_grav at f_C, high-passing Δθ_gyro at the same frequency, and summing the two filtered values, an operation that preserves unity gain and zero phase over all frequencies.
7. For small angles, we can update the quaternion using the operation:
   q₂ = [1;  Δθ_x/2;  Δθ_y/2;  Δθ_z/2] × q₁
8. We can then report rotation rate (ω, measured), orientation (Euler angles derived from q), and translational acceleration (a_trans, from step 3.1) to the SAS main board.

I chose a crossover frequency f_C = 0.1 Hz (0.6 rad/s) as a balance between the evils of offset drift in the rotation rate measurements (mitigated by high crossover frequency) and translational acceleration transients in the gravity vector measurements (mitigated by low crossover frequency).

Software Implementation

The AHRS code is divided into several modules: a main module that calls the necessary functions from the other modules, a kinematic estimator module that implements the algorithm described above, and low-level input, ouput, and math modules.

Main module

One of the funny things the Arduino IDE does is it takes your .pde source files and puts them all into one file and auto-generates a header file before compilation. On the one hand, you don't need to write your own header files, but it also means you can't use keywords like "static" to manage access control (because everything ends up in the same file anyways). So I decided to be lazy and just make all the vector and matrix variables global. They are declared in the main module.

The main module also contains:

setup(): Calls the other modules' initialization functions

loop(): Called whenever the processor is free; it polls the Arduino library millis() function to run a 50 Hz update. The loop body consists of calling the 3 sensor update functions (gyro, accelerometer, and magnetometer), the estimator update function, and finally the data output function.

Estimator module

estimator_init(): Takes an average of a bunch of gravity vector measurements to create a reference vector for the "zero" orientation.

estimator_update(): Calls the low-level math routines to carry out the algorithm described above.

Input modules

I2C module: This is a C library I wrote as an alternative to the Arduino library's supplied I2C module. In order to write data, you supply the module with an I2C address, a pointer to a flag byte, and the data you wish to write. The module copies the data into an internal buffer, begins the writing process (done through hardware), and returns. The ISR makes sure your data gets sent and sets your flag when it's done. To read data, you pass it a pointer to a read buffer. The ISR populates the read buffer as data comes in, and sets your flag when it's done. This way, multiple sources can all access the I2C module at the same time, and nobody has to wait on the hardware.

Sensors module: The sensors are all read and filtered on their own loop (a 1 kHz timer ISR), separate from the main loop. The gyro (an analog input) is read at 500 Hz and the accelerometer (I2C interface) at 200 Hz; both are filtered with an 8 Hz digital lowpass filter. When the user asks for sensor data, the module returns the most recent filtered values (within a global interrupt disable/enable block).

Output modules

SPI Master module: This is a pretty straightforward module implementing blocking code to send a 38-byte data frame from the AHRS to the SAS. Unlike the I2C module, which is called from within an interrupt, it is okay for this code to hold up execution. The data frame consists of a start-of-frame byte, 9 floating point values (Euler angles, gyro rates, and translational accelerations), and a checksum byte.

Serial output module: This is only active in debug mode; it calls the Arduino library Serial.print() functions to send AHRS results and raw sensor readings as ASCII text through the UART module.

Math modules

Vector module: Contains some very straightforward functions for vector addition, scaling, and dot and cross products. Also the fast inverse square root routine, for vector normalization.

Matrix module: Matrix multiplication, and a function for turning a rotation matrix into Euler angles.

Quaternion module: Functions for quaternion scaling and normalization, rotating a quaternion by body-fixed Euler angles, and converting a quaternion into a rotation matrix.

Humidifier

Last week we bought a humidifier (kaz/Vicks Cool Moisture, $30) because we've been experiencing dry skin. I think we've both noticed an improvement since we started using it.

Some details about the humidifier: It functions by blowing air over a wicking filter; claims it can do up to 50% relative humidity for a small/medium room. Currently it puts out liter or so per night; haven't actually quantified this or compared it on the high/low settings. Power consumption is 41.8 W (56.3 VA) on high, 18.4 W (23.1 VA) on low. So running it 10 hours a night for 30 days would be 5.5 (low) or 12.5 (high) kWh/month.