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.