``````// ECE 476 Final Projec: Autonomous stability system for a co-axial Helicopter
// Note: This code has been edited for better
//created Friday, April 7th
// last updated april 21st
// last updated april 23rd
// last updated May 3rd

#include <Mega32.h>
#include <math.h>

// MATH STUFF
// Code Credit for fixed point code: Bruce Land http://www.nbb.cornell.edu/neurobio/land/
//****************************************************************************************

//===The fixed macros=========================================
#define int2fix(a)   (((int)(a))<<8)            //Convert char to fix
#define fix2intSlow(a)  ((signed char)((a)>>8))    //Convert fix to char
#define float2fix(a) ((int)((a)*256.0))         //Convert float to fix
#define fix2float(a) ((float)(a)/256.0)         //Convert fix to float

#define multfixSlow(a,b) ((int)((((long)(a))*((long)(b)))>>8)) //multiply two fixed #
#define divfix(a,b)  ((int)((((long)(a))<<8)/((long)(b)))) //divide two fixed #

//lsqrt is in math.h
#define sqrtfixSlow(a)   (lsqrt(((long)(a))<<8))   //square root

//==Fast fixed multiply=================================
char fix2int(int a)
{
#asm
ldd r30, Y+1 ;moves the high byte of the input to low byte
#endasm
}
//==Fast fixed multiply=================================

int multfix(int a,int b)
{
#asm
;******************************************************************************
;*
;* FUNCTION
;*	muls16x16_24
;* DECRIPTION
;*	Signed multiply of two 16bits numbers with 24bits result.
;* USAGE
;*	r31:r30:rxx = r23:r22 * r21:r20

;******************************************************************************
push r20
push r21

LDD  R22,Y+2  ;load a
LDD  R23,Y+3

LD   R20,Y    ;load b
LDD  R21,Y+1

muls	r23, r21		; (signed)ah * (signed)bh
mov		r31, r0         ;r18, r0
mul		r22, r20		; al * bl
mov     r30, r1         ;movw	r17:r16, r1:r0
;mov     r16, r0
mulsu	r23, r20		; (signed)ah * bl
add		r30, r0         ;r17, r0
adc		r31, r1         ;r18, r1
mulsu	r21, r22		; (signed)bh * al
add		r30, r0         ;r17, r0
adc		r31, r1         ;r18, r1

pop r21
pop r20
#endasm
}

// End fixed point math code
// *********************************

#define minimum 0       // minimum PWM value
#define maxticks 100    // used for PWM
#define maximum 100     // maximum PWM value
#define Smax 60         // Maximum PWM value for stability motors
#define Sinc 256        // Stability Motor PWM increment
#define milsec 160      // used for our "seconds" counter used for
// helicopter timing

// Define sensor channels
#define GYRO_CHANNEL 0
#define AROMX_CHANNEL 1
#define AROMY_CHANNEL 2
#define AROMZ_CHANNEL 3

// Define Helicopter states
#define GROUND 0
#define LANDING 1
#define TAKEOFF 2
#define HOVER 3
#define LEFT 4
#define RIGHT 5
#define FORWARD 6
#define BACKWARD 7

// start off on the ground if we live in reality
char heliState = GROUND;

//used for timing of PWM
int ticks = 0;
char marks = 0;
int msec = 0;
int sec = 0;

// make motor speeds fixed point for greater accuracy
int topRotorSpeed;
int bottomRotorSpeed;
int motorStable1 = 0;
int motorStable2 = 0;
int motorStable3 = 0;

// used sometimes
int avgRotorSpeed = int2fix(0);
int i = 0;
char j,k;
char temp;

char Ain = 0;
char Ainlast = 0;
char first = 0;

// Gyroscope
char gyroCalibrate = 0;  // calibration
char gyro[32];           // buffer
char lastG = 0;          // not used

// X accelerometer
char aromXCalibrate = 0;        // calibration
char aromX[128];                // buffer
char flatnessX = 0;             // not used

// Y accelerometer
char aromYCalibrate = 0;        // calibration
char aromY[128];                // buffer
char flatnessY = 0;             // not used

// Z accelerometer
char aromZCalibrate = 0;        // calibration
char aromZ[128];                // buffer

char currentChannel = 0;

void initialize(void);
void updatePWM(void);
void calibrateGyro(void);
void calibrateArom(void);
char FixFilter32(char f[32]);
char FixFilter64(char f[64]);
char FixFilter128(char f[128]);
char FixFilter256(char f[256]);
char aromZfilter(char f[256]);

interrupt [TIM0_COMP] void pwm(void){
ticks++;
marks++;
if(marks == milsec){
marks = 0;
msec++;
if(msec == 10){
msec = 0;
sec++;
}
}
updatePWM();
}

void main(void){

initialize();

// wait for button press on 1
while(PIND.1 != 0) { }

calibrateGyro();
calibrateArom();

// Lets a GO!

topRotorSpeed = avgRotorSpeed;
bottomRotorSpeed = avgRotorSpeed;

sec = 0;
while(1)
{

// take off
// increase the motor speeds quickly but not abruptly
// so we dont screw up the gears

if(sec <= 5){
heliState = TAKEOFF;
if(topRotorSpeed < int2fix(95)) topRotorSpeed += 16;
if(bottomRotorSpeed < int2fix(95)) bottomRotorSpeed += 16;
}

// hover
// We turn on the X, Y, Z accelerometer stability systems

if(sec == 23) {
heliState = HOVER;
}

// landing
// We turn off all the stability accelerometers
// and their respective motors for stable landing

if(sec == 299){
heliState = LANDING;
motorStable1 = 0;
motorStable2 = 0;
motorStable3 = 0;
}

if(sec >= 300 && sec <= 400){
if(topRotorSpeed > int2fix(50))topRotorSpeed -= 1;
if(bottomRotorSpeed > int2fix(50))bottomRotorSpeed -= 1;
}

// grounded
// We're done! Lets get into an infinite loop so
// watchdog resets the MCU and is ready for the next flight

if(sec >= 410){
heliState = GROUND;
WDTCR=0x08;   //enable watchdog
while(1);     //just wait for watchdog to reset machine
}
}
}

void updatePWM(void){

int tempTick;
tempTick = int2fix(ticks);

// PWM top motor
if (tempTick > topRotorSpeed) PORTC.0 = 0;
else PORTC.0 = 1;

// PWM second motor
if (tempTick > bottomRotorSpeed) PORTC.1 = 0;
else PORTC.1 = 1;

// front motor (1)
if(ticks > fix2int(motorStable1)) PORTC.2 = 0;
else PORTC.2 = 1;

// right motor (2)
if(tempTick > motorStable2) PORTC.3 = 0;
else PORTC.3 = 1;

// left motor (3)
if(tempTick > motorStable3) PORTC.4 = 0;
else PORTC.4 = 1;

//PWM reset etc
if (ticks == maxticks){
ticks = 0;
}

}

Ainlast = Ain;

switch(currentChannel){
// currently looking at the gyro channel
//  clockwise : Ain greater than gyroCalibrate
//  counter-clockwise : Ain is less than gyro calibrate
// for clockwise : top less, bottom more
// for counterclockwise : top more, bottom less

// use of dead zone of +/- 1 from calibrate value
case GYRO_CHANNEL:
currentChannel = AROMX_CHANNEL;
// new conversion

// we decided to compensate for gyroscopic motion
// during takeoff too
//if(heliState != TAKEOFF){

for(i = 0;i < 31; i++){
gyro[i] = gyro[i+1];
}
gyro[31] = Ain;

temp = FixFilter32(gyro);

// originally used different accuracy calibrations
// for take off and hover, and this usually helps somewhat

if(heliState == TAKEOFF) j = 8;
else j = 8;

// we had to use a small threshold since the gyro was a little too sensitive
if(temp < (gyroCalibrate - 1)) {
if(topRotorSpeed < int2fix(maximum)) topRotorSpeed += j;
if(bottomRotorSpeed > int2fix(minimum)) bottomRotorSpeed -= j;
PORTD.7 = 0;
} else if(temp > (gyroCalibrate + 1)) {
if(topRotorSpeed > int2fix(minimum)) topRotorSpeed -= j;
if(bottomRotorSpeed < int2fix(maximum)) bottomRotorSpeed += j;
PORTD.7 = 0;
} else PORTD.7 = 1;
//}

break;

// currently looking at the X axis accelerometer channel
case AROMX_CHANNEL:
currentChannel = AROMY_CHANNEL;
// new conversion
if(heliState == HOVER){
for(i = 0;i < 127; i++){
aromX[i] = aromX[i+1];
}
aromX[127] = Ain;

temp = FixFilter128(aromX);

if(temp < (aromXCalibrate)) {
flatnessX =  aromXCalibrate - temp;
if(motorStable1 > 0) motorStable1 -= Sinc*2;
if(motorStable2 < int2fix(Smax)) motorStable2 += Sinc*2;
if(motorStable3 < int2fix(Smax)) motorStable3 += Sinc*2;
//PORTD.7 = 0;
} else if(temp > (aromXCalibrate + 1)) {
flatnessX = temp - aromXCalibrate;
if(motorStable1 < int2fix(Smax)) motorStable1 += Sinc*2;
if(motorStable2 > 0) motorStable2 -= Sinc*2;
if(motorStable3 > 0) motorStable3 -= Sinc*2;
//PORTD.7 = 0;
} else {
if(motorStable1 > 0) motorStable1 -= Sinc*2;
flatnessX = 0;
//PORTD.7 = 1;
}
}

break;

// currently looking at the Y axis accelerometer channel
case AROMY_CHANNEL:  currentChannel = AROMZ_CHANNEL;
// new conversion

if(heliState == HOVER){
for(i = 0;i < 127; i++){
aromY[i] = aromY[i+1];
}
aromY[127] = Ain;

temp = FixFilter128(aromY);

if(temp < (aromYCalibrate)) {
flatnessY = aromYCalibrate - temp;
if(motorStable2 < int2fix(Smax)) motorStable2 += Sinc*4;
if(motorStable3 > 0 ) motorStable3 -= Sinc*2;

//PORTD.7 = 0;
} else if(temp > (aromYCalibrate)) {
flatnessY = temp - aromYCalibrate;
if(motorStable2 > 0 ) motorStable2 -= Sinc*2;
if(motorStable3 < int2fix(Smax)) motorStable3 += Sinc*4;

//PORTD.7 = 0;
} else {
if(motorStable2 > 0) motorStable2 -= Sinc;
if(motorStable3 > 0) motorStable3 -= Sinc;
flatnessY = 0;
//PORTD.7 = 1;

}
}

break;

// currently looking at the Z axis accelerometer channel
// when going up we have less g so would be lower
// when going down we have more g so should be over
case AROMZ_CHANNEL: currentChannel = GYRO_CHANNEL;
// new conversion

if(heliState == HOVER){
for(i = 0;i < 127; i++){
aromZ[i] = aromZ[i+1];
}
aromZ[127] = Ain;

temp = aromZfilter(aromZ);

// this sensor was a little touchy as one direction was more sensitive
// and noisier than the other.  The threshold allowed us to help this as well
// as prevent the helicopter from losing altitude which was more important than it
// gaining too much altitude

if(temp > (aromZCalibrate + 3)){
if(topRotorSpeed > int2fix(minimum)) topRotorSpeed -= 2;
if(bottomRotorSpeed > int2fix(minimum)) bottomRotorSpeed -= 2;
PORTD.7 = 0;
} else if(temp == (aromZCalibrate)){
if(topRotorSpeed < int2fix(maximum - 3)) topRotorSpeed += 5;
if(bottomRotorSpeed < int2fix(maximum - 3)) bottomRotorSpeed += 5;
PORTD.7 = 0;
} //else PORTD.7 = 1;
}

break;

}

}

void calibrateGyro(void){
//init the A to D converter
//channel zero/ left adj /EXTERNAL Aref
//!!!CONNECT Aref jumper!!!!
// choose AREF = 5

// A0 is the gyro

//enable ADC and set prescaler to 1/64*16MHz=250,000
//and clear interupt enable
//and start a conversion

for(i = 0; i < 31; i++){
while(ADCSR.6 == 1){ }; // do a bunch of conversions
}
while(ADCSR.6 == 1){ }; // take the last one
gyroCalibrate = FixFilter32(gyro);

// get ready for accelerometer calibrate
}

void calibrateArom(void){
// A1 is the AcceleROMeterX (AROMX)
for(i = 0; i < 127; i++){
// do a bunch of conversions
while(ADCSR.6 == 1){ };
}
while(ADCSR.6 == 1){ }; // take the last one
aromXCalibrate = FixFilter128(aromX);

// get ready for next axis

//A2 is AcceleROMeterY (AROMY)

for(i = 0; i < 127; i++){
// do a bunch of conversions
}
while(ADCSR.6 == 1){ }; // take the last one
aromYCalibrate = FixFilter128(aromY);

// reset for operation

// A3 is AcceleROMeterZ  (AROMZ)

for(i = 0; i < 127; i++){
// do a bunch of conversions
while(ADCSR.6 == 1){ };
}
while(ADCSR.6 == 1){ }; // take the last one
aromZCalibrate = FixFilter128(aromZ);

// reset for operation

}

// we coded a number of different filters while we were playing around
// with different filter buffer sizes

char aromZfilter(char f[256]){
int sum = 0;
for(i = 0; i < 256; i++){
sum = sum + f[i];
}

// divide by 256
sum = sum >> 8;

return sum;
}

char FixFilter32(char f[32]){
int sum = 0;
for(i = 0; i < 32; i++){
sum = sum + f[i];
}

// divide by 32
sum = sum >> 5;

return sum;
}

char FixFilter64(char f[64]){
int sum = 0;
for(i = 0; i < 64; i++){
sum = sum + f[i];
}

// divide by 64
sum = sum >> 6;

return sum;
}

char FixFilter128(char f[128]){
int sum = 0;
for(i = 0; i < 128; i++){
sum = sum + f[i];
}

// divide by 128
sum = sum >> 7;

return sum;
}

char FixFilter256(char f[256]){
int sum = 0;
for(i = 0; i < 256; i++){
sum = sum + f[i];
}

// divide by 256
sum = sum >> 8;

return sum;
}

void initialize(void){

// port A input (to ADC)
// port C output to optoisolators
DDRC = 0xff;
// port D all input except for D7
DDRD = 0b10000000;
PORTD = 0xff;

// set up timer0
// we use this to make our own PWM with
// as many channels as we want
TIMSK = 0b00000010;
OCR0 = 100;
TCCR0 = 0b01001001;
#asm
sei
#endasm
}

``````