Woodward "Woody" Stanford
Published

UAV Drone Project

Not content with an RC type drone, I want to design a flying software testing platform with which to develop new guidance methodologies.

AdvancedWork in progressOver 41 days9,641
UAV Drone Project

Things used in this project

Hardware components

BeagleBone Black Wireless
BeagleBoard.org BeagleBone Black Wireless
×1
NEO-6M GPS Module
×1
GY-521 MPU-6050 3 Axis Gyroscope + Accelerometer Module For Arduino
×1
GE Hobby ESC - SimonK Firmware - 30 Amp
×8
PIC16F
Microchip PIC16F
×1
3000KV 3-Phase Drone Motor
×8

Software apps and online services

MPLAB
VB.NET
GNU C/C++ (ANSI/POSIX-Compliant-Shell Access)

Hand tools and fabrication machines

GE Fiscars Drill

Story

Read more

Schematics

Drone Block Diagram 1

Semi-completed drone

Hookup diagram of a PicKit3 to a PIC16F88

Here is a PDF file with some of the hookup diagrams I use for programming my custom PWM exciter chip as well as mapping out the UART chanels on my BBBW.

Code

Sample FMM model

C/C++
*** IT DOESN'T WORK *** however if you walk thru it you'll see a lot of ideas to code your own. A lot of stuff in it does: vector libraries, equations, approaches. But a must read for someone that's looking to code one of these themselves.

OK, if you want to know how it works, its contained in this ONE comment line:
//how the model is practically used is through the manipulation of the ESC array

You programatically adjust the power on all the drone motors and the model computes how the drone moves over time. Its up to you how u do this: you can (1) hook up a USB PS3 controller, (2) you can code an actual guidance algorithm, (3) load it from another external tool, (4) you can tweak it by hand.
#include <stdio.h>
#include <stdlib.h>
#include <string.h> //required for graph3
#include <math.h>

//Stanford Systems graphics support
#include "ssmotor_graph3.c"

//state machine constants
#define OFF 0
#define ON 10
#define ASCEND 20
#define AQUIRE 30
#define TRAVEL 40
#define NEUTRAL 50
#define DESCEND 60

//common duty cycles
//set empirically with comfortable values
#define OFF_DC 0.00
#define IDLE_DC 0.10
#define ASCEND_DC 0.30 
#define DESCEND_DC 0.20

int curstate=OFF; //current system state

int go_signal=0; //GO signal to initiate flight

long glng_counter=0L; //global generic counter

//force-moment-mass model
struct fmm_type
{
	double f1a,f1b; //motor pod 1 - in Newtons (at 100% dc)
	double m1x,m1y,m1z; //center of pod 1 - in meters 
	double f2a,f2b; //motor pod 2 - in N (at 100% dc)
	double m2x,m2y,m2z; //center of pod 2 - in m
	double f3a,f3b; //motor pod 3 - in N (at 100% dc)
	double m3x,m3y,m3z; //center of pod 3 - in m
	double f4a,f4b; //motor pod 4 - in N (at 100% dc)
	double m4x,m4y,m4z; //center of pod 4 - in m

	double weight; //in kg
	double cgx,cgy,cgz; // in m - origin is (0,0,0)
	double ip,iy,ir; //moments of inertia around CG

	//this value is the force of a single motor/prop at 100% throttle
	//Determined empirically - in Newtons	
	double mfce; //motor force coefficient

} fmm;

struct curdrone_type
{
	//curdrone hangs onto all of the current variables of the drone

	double x,y,z; //current drone position - in meters
	double vx,vy,vz; //current drone velocity - in meters/sec
	double ax,ay,az; //current drone acceleration - in meters/sec^2

	double tp,tr,ty; //current drone orientation - in radians
	double vtp,vtr,vty; //current angular velocity - in radians/sec
	double atp,atr,aty; //current angular acceleration - in radians/sec^2
	//tp = theta pitch, tr = theta roll, ty=theta yaw
	double weight; //current drone weight

} curdrone;

//current ESC values
struct esc_type
{
	double dutycycle; //percentage (0.00<=x<=1.00) of full duty cycle 
} esc[8];

//general globals
double batt_voltage=1.2*6.0; //voltage output of battery pack - in volts 
double batt_current=0.00; //maximum current of battery pack - in amps
double batt_energy=2.5*6.0; //total energy of charged battery pack - in amp*hours

double cur_batt_energy=0.00; //current energy left in battery pack - in amp*hours

int done=0; //main loop flag

initialize_fmm()
{
	//load the model data

	//motor pod 1
	fmm.f1a=0.00; //in newtons
	fmm.f1b=0.00; //in N
	fmm.m1x = 0.00; //in meters
	fmm.m1y = 0.00; //in m
	fmm.m1z = 0.00; //in m

	//motor pod 2
	fmm.f2a=0.00; //in newtons
	fmm.f2b=0.00; //in N
	fmm.m2x = 0.00; //in meters
	fmm.m2y = 0.00; //in m
	fmm.m2z = 0.00; //in m

	//motor pod 3
	fmm.f3a=0.00; //in newtons
	fmm.f3b=0.00; //in N
	fmm.m3x = 0.00; //in meters
	fmm.m3y = 0.00; //in m
	fmm.m3z = 0.00; //in m

	//motor pod 4
	fmm.f4a=0.00; //in newtons
	fmm.f4b=0.00; //in N
	fmm.m4x = 0.00; //in meters
	fmm.m4y = 0.00; //in m
	fmm.m4z = 0.00; //in m

	//mass
	fmm.weight=0.00; //in kg

	//moments of inertia
	//how much force is required to rotate
	fmm.ip=0.00; //moment of pitch
	fmm.iy=0.00; //moment of yaw
	fmm.ir=0.00; //moment of roll

	//other var
	fmm.mfce=100.0; //motor force coefficient - in Newtons (at DC=100%)

	//initialize curdrone
	//curdrone contains the system's current values (used directly for flight control)
	//this system currently uses inertial only so its coordinate system is relative
	//to align the drone for absolute, make sure its on level ground AND pointing due East

	//at startup we calibrate based on its initial position and orientation
	curdrone.x=0.0; curdrone.y=0.0; curdrone.z=0.0; //this is origin defined as where we start
	curdrone.vx=0.0; curdrone.vy=0.0; curdrone.vz=0.0; //velocity is zero since it just sitting there	
	curdrone.ax=0.0; curdrone.ay=0.0; curdrone.az=-9.8; //acceleration is zero since it is just sitting there
	//except for gravity which is always 9.8 m/s^2 down

	//transfer inital weight value to current value (use this for actual flight)
	curdrone.weight=fmm.weight;


}

struct waypoint_type
{
	double x,y,z; //absolute coordinates of waypoint relative to origin - in Meters
	//x is East/West (positive is East)
	//y is North/South (positive is North)
	//z is Altitude (positive is Up)
	
};

struct fplan_type
{
	double initial_altitude;
	struct waypoint_type waypoint[1000]; //flight plan allows for up to 1000 waypoints
	int num_waypoints;
} fplan;	

initialize_flightplan()
{
	//Note: current system only supports one flight plan (due to large size of waypoint array)
	int a;
	long fp1[][3] = {	{0.0,0.0,0.0},{0.0,0.0,0.0},{0.0,0.0,0.0},{0.0,0.0,0.0},{0.0,0.0,0.0},
				{0.0,0.0,0.0},{0.0,0.0,0.0},{0.0,0.0,0.0},{0.0,0.0,0.0},{0.0,0.0,0.0}};

	fplan.initial_altitude=100.0; //ASCENDS to this altitude on take off - in Meters
	fplan.num_waypoints = 10; //number of waypoints - remember to make the final waypoint (0.0,0.0,initial_altitude)

	//in C, arrays aren't zeroed out. Do that now
	for (a=0;a<1000;a++)
	{
		fplan.waypoint[a].x=0.00;		
		fplan.waypoint[a].y=0.0;
		fplan.waypoint[a].z=0.0;
	}
		
	//load flight plan from loading array, fp1
	for (a=0;a<fplan.num_waypoints;a++)
	{
		fplan.waypoint[a].x=fp1[a][0];		
		fplan.waypoint[a].y=fp1[a][1];
		fplan.waypoint[a].z=fp1[a][2];
	}

	//DEBUG: save a copy of flight plan to filesystem
	save_flightplan("default.dfp");

	return; //flight plan loaded
}

save_flightplan(char *fname)
{
	int a;
	FILE *fptr;

	//how we support multiple flight plans is by loading/saving to filesystem
	//for now I just want to be able to save one

	fptr=fopen(fname,"w");

	fprintf(fptr,"%2.2lf\n",fplan.initial_altitude);
	fprintf(fptr,"%d\n",fplan.num_waypoints);

	for (a=0;a<fplan.num_waypoints;a++)
		fprintf(fptr,"%2.2lf, %2.2lf, %2.2lf\n",fplan.waypoint[a].x,fplan.waypoint[a].y,fplan.waypoint[a].z);

	fclose(fptr);
}

load_flightplan(char *fname)
{
	//code later
	return 0; //flight plan NOT loaded
}

// *** 3D VECTOR TYPES ***
struct vector3D_type
{
	double x,y,z; //cartesian (x,y,z) in unitless, unitless, unitless
	//why unitless, so we can use it for velocity, acceleration and force
} tmp13D, tmp23D, tmp33D;

struct vector3DP_type
{
	double d,t1,t2; //polar (amplitude, theta1, theta2) in unitless, rad, rad
	//why unitless? so we can use it for various units dependant on context
} tmp13DP, tmp22DP, tmp33DP; //polar versions have a P suffix

makevector3D(struct vector3D_type *v, double x, double y, double z)
{
	v->x=x;
	v->y=y;
	v->z=z;
}

fmm_model(double timeslice) //used for actual flight control
{
	//timeslice is the amount of time that the model is being computed over (usually
	// around 100 milliseconds, or 0.1 seconds) - in secs 
	//necessary for correct integration/differentiation (ie. numeric analysis)

	//ANGULAR variables
	//pitch torque scalar for each motor 
	double tpm1a, tpm1b, tpm2a, tpm2b,tpm3a, tpm3b, tpm4a, tpm4b;
	//roll torque scalar for each motor 
	double trm1a, trm1b, trm2a, trm2b,trm3a, trm3b, trm4a, trm4b;

	double daap; //delta angular acceleration on pitch
	double daar; //delta angular acceleration on roll
	//yaw is computed differently using precession

	//FORCE variables
	double fxm1a,fxm1b,fxm2a,fxm2b,fxm3a,fxm3b,fxm4a,fxm4b;
	double fym1a,fym1b,fym2a,fym2b,fym3a,fym3b,fym4a,fym4b;
	double fzm1a,fzm1b,fzm2a,fzm2b,fzm3a,fzm3b,fzm4a,fzm4b;
	double tff,daf;
	double tfu, dau;
	double tfr,dar;

	double fw; //force of drone's weight down by gravity

	//how the model is practically used is throught the manipulation of the ESC array
	//which has the current dutycycle (ie. throttle) values for each motor. The model
	//reacts to the current values of the motors' power causing it to move, rotate around
	//an given axis, or to rise or fall.

	//motor force vectors
	struct vector3D_type fm1a3D, fm1b3D, fm2a3D, fm2b3D, fm3a3D, fm3b3D, fm4a3D, fm4b3D;

	//calculate force down due to mass
	//represented scalar but is actually a 3D vector, direction down absolute
	fw=curdrone.weight*9.8;   //F=ma, F=mg - In Newtons

	//note: the motor force coefficient is pre-calculated empirically - in Newtons
	
	//calculate motor forces based on current throttle
	//these are represented as scalars but are actually 3D vectors, direction UP relative
	fmm.f1a=esc[0].dutycycle*fmm.mfce; //the esc array is our master reference for motor force calc
	fmm.f1b=esc[1].dutycycle*fmm.mfce;
	fmm.f2a=esc[2].dutycycle*fmm.mfce;
	fmm.f2b=esc[3].dutycycle*fmm.mfce;
	fmm.f3a=esc[4].dutycycle*fmm.mfce;
	fmm.f3a=esc[5].dutycycle*fmm.mfce;
	fmm.f4a=esc[6].dutycycle*fmm.mfce;
	fmm.f4a=esc[7].dutycycle*fmm.mfce;

	//make motor force vectors
	makevector3D(&fm1a3D,0.0,0.0,fmm.f1a);
	makevector3D(&fm1b3D,0.0,0.0,fmm.f1b);
	makevector3D(&fm2a3D,0.0,0.0,fmm.f2a);
	makevector3D(&fm2b3D,0.0,0.0,fmm.f2b);
	makevector3D(&fm3a3D,0.0,0.0,fmm.f3a);
	makevector3D(&fm3b3D,0.0,0.0,fmm.f3b);
	makevector3D(&fm4a3D,0.0,0.0,fmm.f4a);
	makevector3D(&fm4b3D,0.0,0.0,fmm.f4b);

	//note that we don't have the center of each propeller but its pod (an approximation but should work)
	//calculate pitch torques
	//use Stanford component approach (verify that it works)
	tpm1a=fm1a3D.z*cos(atan(fmm.m1y/fmm.m1x)) * pow(pow(fmm.m1x,2.0)+pow(fmm.m1y,2.0),0.5); //t=f*d
	tpm1b=fm1a3D.z*cos(atan(fmm.m1y/fmm.m1x)) * pow(pow(fmm.m1x,2.0)+pow(fmm.m1y,2.0),0.5); 
	tpm2a=fm1a3D.z*cos(atan(fmm.m1y/fmm.m1x)) * pow(pow(fmm.m1x,2.0)+pow(fmm.m1y,2.0),0.5);
	tpm2b=fm1a3D.z*cos(atan(fmm.m1y/fmm.m1x)) * pow(pow(fmm.m1x,2.0)+pow(fmm.m1y,2.0),0.5);
	tpm3a=fm1a3D.z*cos(atan(fmm.m1y/fmm.m1x)) * pow(pow(fmm.m1x,2.0)+pow(fmm.m1y,2.0),0.5);
	tpm3b=fm1a3D.z*cos(atan(fmm.m1y/fmm.m1x)) * pow(pow(fmm.m1x,2.0)+pow(fmm.m1y,2.0),0.5);
	tpm4a=fm1a3D.z*cos(atan(fmm.m1y/fmm.m1x)) * pow(pow(fmm.m1x,2.0)+pow(fmm.m1y,2.0),0.5);
	tpm4b=fm1a3D.z*cos(atan(fmm.m1y/fmm.m1x)) * pow(pow(fmm.m1x,2.0)+pow(fmm.m1y,2.0),0.5);

	//calculate roll torques
	trm1a=fm1a3D.z*cos(atan(fmm.m1z/fmm.m1y)) * pow(pow(fmm.m1z,2.0)+pow(fmm.m1y,2.0),0.5); //t=f*d
	trm1b=fm1a3D.z*cos(atan(fmm.m1z/fmm.m1y)) * pow(pow(fmm.m1z,2.0)+pow(fmm.m1y,2.0),0.5); 
	trm2a=fm1a3D.z*cos(atan(fmm.m2z/fmm.m1y)) * pow(pow(fmm.m2z,2.0)+pow(fmm.m2y,2.0),0.5);
	trm2b=fm1a3D.z*cos(atan(fmm.m2z/fmm.m1y)) * pow(pow(fmm.m2z,2.0)+pow(fmm.m2y,2.0),0.5);
	trm3a=fm1a3D.z*cos(atan(fmm.m3z/fmm.m1y)) * pow(pow(fmm.m3z,2.0)+pow(fmm.m3y,2.0),0.5);
	trm3b=fm1a3D.z*cos(atan(fmm.m3z/fmm.m1y)) * pow(pow(fmm.m3z,2.0)+pow(fmm.m3y,2.0),0.5);
	trm4a=fm1a3D.z*cos(atan(fmm.m4z/fmm.m1y)) * pow(pow(fmm.m4z,2.0)+pow(fmm.m4y,2.0),0.5);
	trm4b=fm1a3D.z*cos(atan(fmm.m4z/fmm.m1y)) * pow(pow(fmm.m4z,2.0)+pow(fmm.m4y,2.0),0.5);

	//calculate delta angular accelerations (pitch and roll)
	daap=(tpm1a+tpm1a+tpm1a+tpm1a+tpm1a+tpm1a+tpm1a+tpm1a)/fmm.ip; //pitch angular acceleration in rad/sec^2
	daar=(tpm1a+tpm1a+tpm1a+tpm1a+tpm1a+tpm1a+tpm1a+tpm1a)/fmm.ip; //roll angular acceleration in rad/sec^2

	//you don't have to calculate yaw torques because they don't exist with this drone design

	//how we are going to control yaw is by running the motors at 90% and then we adjust the counter-rotation
	//between 80%/100% and 100%/80%. It should result in an angular rate of "precession" (like a top) that is
	//calculatable.

	//update curdrone variables with new results

	//change angular acceleration
	curdrone.atp += daap;
	curdrone.atr += daar; 
	//calculate angular velocity from angular acceleration
	curdrone.vtp+=curdrone.atp * timeslice; //timeslice is necessary for proper integration
	curdrone.vtr+=curdrone.atr * timeslice; //timeslice is necessary for proper integration
	//calculate orientation from angular velocity
	curdrone.tp+=curdrone.vtp * timeslice;
	curdrone.tr+=curdrone.vtr * timeslice;


	//calculate x direction force vectors
	fxm1a=fm1a3D.z * sin(curdrone.tp);
	fxm1b=fm1b3D.z * sin(curdrone.tp);
	fxm2a=fm2a3D.z * sin(curdrone.tp);
	fxm2b=fm2b3D.z * sin(curdrone.tp);
	fxm3a=fm3a3D.z * sin(curdrone.tp);
	fxm3b=fm3b3D.z * sin(curdrone.tp);
	fxm4a=fm4a3D.z * sin(curdrone.tp);
	fxm4b=fm4b3D.z * sin(curdrone.tp);
	
	//total force forward
	tff=fxm1a+fxm1b+fxm2a+fxm2b+fxm3a+fxm3b+fxm4a+fxm4b;

	//delta acceleration forward
	daf=tff*curdrone.weight; //f=m*a

	//calculate y direction force vectors
	fym1a=fm1a3D.z * sin(curdrone.tr);
	fym1b=fm1b3D.z * sin(curdrone.tr);
	fym2a=fm2a3D.z * sin(curdrone.tr);
	fym2b=fm2b3D.z * sin(curdrone.tr);
	fym3a=fm3a3D.z * sin(curdrone.tr);
	fym3b=fm3b3D.z * sin(curdrone.tr);
	fym4a=fm4a3D.z * sin(curdrone.tr);
	fym4b=fm4b3D.z * sin(curdrone.tr);

	//total force right
	tfr=fym1a+fym1b+fym2a+fym2b+fym3a+fym3b+fym4a+fym4b;

	//delta acceleration right
	dar=tfr*curdrone.weight; //f=m*a


	//calculate z direction force vectors
	fzm1a=fm1a3D.z * cos(curdrone.ty);
	fzm1b=fm1b3D.z * cos(curdrone.ty);
	fzm2a=fm2a3D.z * cos(curdrone.ty);
	fzm2b=fm2b3D.z * cos(curdrone.ty);
	fzm3a=fm3a3D.z * cos(curdrone.ty);
	fzm3b=fm3b3D.z * cos(curdrone.ty);
	fzm4a=fm4a3D.z * cos(curdrone.ty);
	fzm4b=fm4b3D.z * cos(curdrone.ty);



	//total force up
	tfu=fzm1a+fzm1b+fzm2a+fzm2b+fzm3a+fzm3b+fzm4a+fzm4b-fw; //notice we are subtracting the gravity force HERE!

	//delta acceleration up
	dau=tfu*curdrone.weight; //f=m*a

	//update curdrone variables with new results
	//change acceleration
	curdrone.ax += daf;
	curdrone.ay += dar;
 	curdrone.az += dau;
	//calculate velocity from acceleration
	curdrone.vx+=curdrone.ax * timeslice; //timeslice is necessary for proper integration
	curdrone.vy+=curdrone.ay * timeslice; 
	curdrone.vz+=curdrone.az * timeslice;
	//calculate position from velocity
	curdrone.x+=curdrone.vx * timeslice; //timeslice is necessary for proper integration
	curdrone.y+=curdrone.vy * timeslice; 
	curdrone.z+=curdrone.vz * timeslice;

}

setmotor(int motor, double dutycycle)
{
	//kind of a trivial function but...
	esc[motor].dutycycle=dutycycle;
}

setmotors(double dutycycle)
{
	esc[0].dutycycle=dutycycle;
	esc[1].dutycycle=dutycycle;
	esc[2].dutycycle=dutycycle;
	esc[3].dutycycle=dutycycle;
	esc[4].dutycycle=dutycycle;
	esc[5].dutycycle=dutycycle;
	esc[6].dutycycle=dutycycle;
	esc[7].dutycycle=dutycycle;
}

killmotors()
{
	setmotors(0.0);
}

idlemotors()
{
	setmotors(IDLE_DC);
}

main()
{
	printf("DRONED v0.89 drone flight control daemon\n");

	printf("Initializing graph3 graphics support...");
	initgraph3();
	setcolor(0,255,255,255);
	textout(0,50,20,"Drone Flight Simulation",2);
	printf("Initialized\n");

	printf("Initializing drone mathematical model...");
	initialize_fmm(); //load the fmm model into memory
	printf("initialized.\n");

/* Description of Flight Algorythm

	Every flight can be described in terms of the states necessary to complete the "flight plan".

	Initially, we are in an OFF condition (motors @ 0 RPM, on the ground, electronics still powered however).
	Upon receiving the signal to GO, we change state to ON (motors @ low idle) and when preflight self-tests
	are completed, we change state to ASCEND. It goes straight up to its "initial altitude".

	Once initial altitude is acheived, we enter the "travel loop" or start processing the flight plan. The
	first waypoint coordinates are loaded as the current waypoint coordinates. We go to the AQUIRE state
	which points the drone at the current waypoint.	As soon as we are pointed at the current waypoint we
	enter the TRAVEL state where it executes the guidance algorytm that causes the drone to travel to the
	current waypoint.

	Once it reaches that waypoint, it loads the next waypoint (setting the new coordinates as the current
	waypoint) and enters the AQUIRE mode. Once it is pointing directly at the current waypoint, it again
	enters the TRAVEL state, and so on and so forth.

	Once the final waypoint has been acheived, the state is set to NEUTRAL which cause the drone to stop
	in mid-air. Once its stopped, the state changes to DESCEND which is the reverse of the ASCEND process.
	It drops in altitude until it touches down on the ground. Once it is no longer descending, it enters
	the OFF state, its flight plan having been executed.

	Defintion: Flight Plan: since we will be piloting entirely off of inertial, we look at the flight plan
	as an array of waypoints that are expressed as x/y/z delta distances (measured in meters) from the initial location
	of the drone. The flight plan is loaded into the fplan array and its waypoints processed.

	Additional Info: GPS data will be logged to filesystem in unprocessed NMEA format at 1 Hz (to conserve memory).
	Processing of this log file is post-flight by transfer of the file to PC for post-analysis.

	Orientation: the drone's take-off location is defined as origin (0,0,0) and the original yaw orientation as
	theta yaw = 0 degrees. Of course this leads to the realization that the flight plan is entirely relative, yet
	it makes sense.

*/

	printf("Initializing Flight Plan...");
	initialize_flightplan();
	printf("Initialized.\n");

	//DEBUG: we need to set the go_signal to advance curstate so everything works
	go_signal=1;

	printf("Entering main loop...\n");
	while (!done)
	{

		curstate=OFF; //debug: make sure entire system is OFF by default

		switch(curstate)
		{
			case OFF: //kill motors

				killmotors();

				if (go_signal) curstate=ON;

			break;

			case ON: //bring motors to low idle

				idlemotors();

				//sit there idling for about 5 seconds to warm up motors
				if (glng_counter==0) glng_counter=50;
				glng_counter--;
				if (glng_counter==0) curstate=ASCEND;

			break;

			case ASCEND: //take off vertically to initial altitude

				setmotors(ASCEND_DC);
				
				if (curdrone.y>=fplan.initial_altitude) curstate=DESCEND;

			break;

			case AQUIRE: //precess so facing next waypoint


			break;

			case TRAVEL: //continue to next way point


			break;
			
			case NEUTRAL: //stop in the air


			break;

			case DESCEND: //land vertically to zero altitude

				setmotors(DESCEND_DC);

				if (curdrone.y<=0)
				{
					killmotors();
					curstate=OFF;
					done=1; //exit loop so we can save our simulation BMP
				}

			break;

		}

		//update drone model
		//fmm_model(0.100); //timeslice: 100 milliseconds (10 Hz)

		//DEBUG: output current drone position to stdout
		printf("x=%2.2lf, y=%2.2lf, z=%2.2lf...\n",curdrone.x,curdrone.y,curdrone.z);

		//update graphics
		setcolor(0,255,255,255);
		bar2(0,curdrone.x*5+(640/2), (480/2)-curdrone.y*5,1,1);

		//debug
		done=1;

	}

	printf("Writting graphical representation of flight to flight.bmp...");
	writebmp("flight.bmp",0);
	printf("done.\n");

	printf("\nDaemon shutting down...returning to OS\n\n");
	return 0;

}

Sample Code for S3 serial-to-PWM chip

Assembly x86
*** DOESN'T WORK *** but a strong place to start coding your PIC16F88 chip (has both the UART in code and the service routine for doing hobby servo/ESC type PWM out on 8 hannels). It is in assembly (usues cycle counting to get the PWM PERFECT!). It will build however without errors in MPLAB X (freeware from microchip.com) to an actual programable HEX file, it just doesn't work right yet.
  list        p=16f88   ; list directive to define processor
  #include    <p16f88.inc>   ; processor specific variable definitions

  __CONFIG    _CONFIG1, _LVP_OFF & _FCMEN_ON & _IESO_OFF & _CPD_OFF & _CP_OFF & _MCLRE_OFF & _PWRTE_ON & _WDT_OFF 
  __CONFIG    _CONFIG2, _WRT_OFF 

    org 0x0000                     ;start at 0x0000

;important note: did you know you can set your general purpose registers to
; 0x70h to 0x7F and it doesn't matter the bank settings, they always resolve
    
;current internal values for the pwm signals (8 bit)
VALUE0 EQU 0x20 ;not used; padding so indirect addr, hits right channel number
VALUE1 EQU 0x21 ; remember to set the bank to Bank 0 to access correctly 
VALUE2 EQU 0x22
VALUE3 EQU 0x23
VALUE4 EQU 0x24
VALUE5 EQU 0x25
VALUE6 EQU 0x26
VALUE7 EQU 0x27
VALUE8 EQU 0x28
LED1 EQU 0x29
LED2 EQU 0x2A
LED3 EQU 0x2B
LED4 EQU 0x2C
LED5 EQU 0x2D

;generic registers for loop counting
COUNT1 EQU 0x30
COUNT2 EQU 0x31
 
;Register Restore vars
CACHEW EQU 0x70		;save W register HERE for restore on subroutine exit
			;note that it doesn't matter how your banks are set
PROSTATE EQU 0x71	;packet receive state, 0=awaiting first channel byte, 1=awating value byte
			;note that it doesn't matter how your banks are set
PROCHANNEL EQU 0x72	;received channel value
			;note that it doesn't matter how your banks are set
PROVALUE EQU 0x73	;received value value
			;note that it doesn't matter how your banks are set

;define values not in the INC file that we need
SYNCH EQU 0x04		;SYNCH is bit 4 of TXSTA (clearing enables Asynch tranmission/reception)
SPEN EQU 0x07		;SPEN is bit 7 of RCSTA (setting enables UART)
CREN EQU 0x04		;CREN is bit 4 of RCSTA (setting enables receiving bytes on UART RX)
TXEN EQU 0x05		;TXEN is bit 5 of TXSTA (setting enables transmitting bytes on UART TX
BRGH EQU 0x02		;BRHG is bit 2 of TXSTA (setting enables high-speed options on baud rate)
			;(reference baud rate tables in PIC manual for how to set SPBRG for baud rate)
			
initalize
	
    movlw 0x00	    ;initialize protocol state to state 0
    movwf PROSTATE
    
port_config
    
    call selbank1
    movlw b'00000000'           ;set PORTA to output - 8 software PWM channels
    movwf TRISA
    movlw b'00000100'           ;set RB1,3-7 on PORTB to output (except RB2 
    movwf TRISB		        ;which we want to use for UART RX, input)
    
    call selbank0		;select Registers at Bank 0
				;important to flip back to Bank 0 because this
				;is where our variables are
	
uart_config
		
    bsf TXSTA, BRGH		;set UART for high baud rate
				
    movlw 0x81	    ;decimal 129 corresponds to 9600 baud when PIC is running at 20 Mhz
		    ;maximum baud rate for a 20 Mhz 16F88 is 1.25Mbaud SPBRG=0
		    
    call selbank1
    movwf SPBRG	    ;set the baud rate
    
    bcf TXSTA, SYNCH ; clear synch bit for ASYNCH reception/transmission
    bsf RCSTA, SPEN ; enable UART
    
    ;code later ;set up PortB-2 (Pin 8) for UART RX
    
    bsf RCSTA, CREN ; enable reception
    bcf RCSTA, TXEN ; disable transmission
    
    ; Bank 0,PIR,RCIF indicates data is ready for read
    ; Bank 0, RCREG read byte from this register
    
    call selbank0		;select Registers at Bank 0
				;important to flip back to Bank 0 because this
				;is where our variables are
				
debug1	; remove from program when actually writing to chip (for ICE only)
    movlw 0x00	    ;set VALUE1 -> VALUE 8 with a time delay proportional 
    movwf VALUE1    ;to their channel number, allows you to walk the servo
    movlw 0x20	    ;or ESC signal pin over the PIC pins and watch it move
    movwf VALUE2    ;incrementally illustrating that the PWM is set up correctly
    movlw 0x40
    movwf VALUE3
    movlw 0x60
    movwf VALUE4
    movlw 0x80
    movwf VALUE5
    movlw 0xA0
    movwf VALUE6
    movlw 0xC0
    movwf VALUE7
    movlw 0xE0
    movwf VALUE8
    
    movlw 0x01	    ;set LED's alternating ON steady
    movwf LED1
    movlw 0x00	    
    movwf LED2
    movlw 0x01	    
    movwf LED3
    movlw 0x00	    
    movwf LED4
    movlw 0x01	    
    movwf LED5
    
    ;end of debug - remove in actual production version! (maybe, can serve as a preconfig)
    
start
;to implement software-based 8 channel PWM of the hobby servo kind
;as the 3-phase drone motors we are programming for
;it is pretty off-cycle tolerant but we need to correctly time
; the ON time of the PWM on that particular channel
    
;PWM specification:
; 1 milliseconds on - left-most position (0 RPM on 3-phase drone motor)
; 1.5 milliseconds on - center position (middle RPM on 3-phase drone motor)
; 2 milliseconds on - right-most position (Highest RPM on 3-phase drone motor)
; off time doesn't matter that much so long as its refreshed every once in a while
; guessing every 40 milliseconds
    
; instruction time on this 4 x clock cycle time or 0.2 usecs or 200 nanoseconds
;(assuming MCU clock by XTAL at 20 Mhz)
    
;how we build the waveform is by calling a standarized 1 millisecond
;delay (delay1) after throwing the channel's pin HIGH; then we call delay2
;with the CHANNEL NUMBER (NOT the delay value) which completes the rest of
; the waveform (basicaly a linear, programable fraction of 1 millisecond)
    
    call selbank0	;make sure VALUE1->8 are visible
    
    
    bsf PORTA, 0	; throw channel #1 (PORTA-Pin 1) HIGH
    call delay1		; call the 1 ms delay	
    movfw VALUE1	; servicing CHANNEL 1
    call delay2		; W is already loaded with the channel #
			; calls the actual delay value from VALE1 - VALUE 8
			; which is set by the UART routines
    bcf PORTA, 0	; throw channel #1 (PORTA-Pin 1) LOW
    
    
    bsf PORTA, 1	; throw channel #2 (PORTA-Pin 2) HIGH
    call delay1		; call the 1 ms delay
    movfw VALUE2	; servicing CHANNEL 2
    call delay2		; W is already loaded with the channel #
			; calls the actual delay value from VALUE1 - VALUE 8
			; which is set by the UART routines
    bcf PORTA, 1	; throw channel #2 (PORTA-Pin 2) LOW
    
    bsf PORTA, 2	; throw channel #3 (PORTA-Pin 3) HIGH
    call delay1		; call the 1 ms delay	
    movfw VALUE3	; servicing CHANNEL 3
    call delay2		; W is already loaded with the channel #
    bcf PORTA, 2	; throw channel #3 (PORTA-Pin 3) LOW
    
    bsf PORTA, 3	; throw channel #4 (PORTA-Pin 4) HIGH
    call delay1		; call the 1 ms delay	
    movfw VALUE4	; servicing CHANNEL 4
    call delay2		; W is already loaded with the channel #
    bcf PORTA, 3	; throw channel #4 (PORTA-Pin 4) LOW
    
    bsf PORTA, 4	; throw channel #5 (PORTA-Pin 5) HIGH
    call delay1		; call the 1 ms delay	
    movfw VALUE5	; servicing CHANNEL 5
    call delay2		; W is already loaded with the channel #
    bcf PORTA, 4	; throw channel #1 (PORTA-Pin 5) LOW
    
    bsf PORTA, 5	; throw channel #6 (PORTA-Pin 6) HIGH
    call delay1		; call the 1 ms delay	
    movfw VALUE6	; servicing CHANNEL 6
    call delay2		; W is already loaded with the channel #
    bcf PORTA, 5	; throw channel #6 (PORTA-Pin 6) LOW
    
    bsf PORTB, 0	; throw channel #1 (PORTA-Pin 7) HIGH
    call delay1		; call the 1 ms delay	
    movfw VALUE7	; servicing CHANNEL 7
    call delay2		; W is already loaded with the channel #
    bcf PORTB, 0	; throw channel #1 (PORTA-Pin 7) LOW
    
    bsf PORTB, 1	; throw channel #1 (PORTA-Pin 8) HIGH
    call delay1		; call the 1 ms delay	
    movfw VALUE8	; servicing CHANNEL 8
    call delay2		; W is already loaded with the channel #
    bcf PORTB, 1	; throw channel #8 (PORTA-Pin 8) LOW
    
    ; **** Beginning of LED service routine
    ; we need to be able to BOTH turn them on and off programatically via UART
    btfsc LED1,0
    goto led1_on
    bcf PORTB,3	;throw LED #1 (PORTB_3 - Pin 9) LOW
    goto continue_led2
    
led1_on
    bsf PORTB,3	;throw LED #1 (PORTB_3- Pin 9) HIGH
 
continue_led2 
    btfsc LED2,0
    goto led2_on
    bcf PORTB,4	;throw LED #2 (PORTB_4- Pin 10) LOW
    goto continue_led3
    
led2_on
    bsf PORTB,3	;throw LED #2 (PORTB_4- Pin 10) HIGH
    
continue_led3 
    btfsc LED3,0
    goto led3_on
    bcf PORTB,5	;throw LED #3 (PORTB_5- Pin 11) LOW
    goto continue_led4
    
led3_on
    bsf PORTB,5	;throw LED #3 (PORTB_5- Pin 11) HIGH
    
continue_led4
    btfsc LED4,0
    goto led4_on
    bcf PORTB,6		;throw LED #4 (PORTB_6- Pin 12) LOW
    goto continue_led5
    
led4_on
    bsf PORTB,6		;throw LED #4 (PORTB_6 - Pin 12) HIGH
    
continue_led5
    btfsc LED5,0
    goto led5_on
    bcf PORTB,7		;throw LED #5 (PORTB_7 - Pin 13) LOW
    goto continue_led_finished
    
led5_on
    bsf PORTB,7		;throw LED #5 (PORTB_7 - Pin 13) HIGH
    
continue_led_finished	;end of LED servicing
   
uart_service
    ;implement state machine (
    ;   0 = waiting for channel byte,
    ;   1 = waiting for value byte,
    ;	2 = "frame" received, process it! )
    
    ; Bank 0,PIR,RCIF indicates data is ready for read
    ; Bank 0, RCREG read byte from this register
    
prostate0
    
    movfw PROSTATE	    ;remember this "corrupts" W
    sublw 0x00		    ;sets the zero flag if PROSTATE is 0 (ie. state 0)
    btfss STATUS, Z
    goto prostate1	    ;proceeds to the next state test if not this state 
    
    ;is there a channel byte ready for reception?
    call selbank0
    btfss PIR1,RCIF	    ;if bit set indicates data is ready for read
    goto prostate1    ; proceeds without processing if channel byte hasn't hit yet
	
    movfw RCREG	    ;move received byte into W
    movwf PROCHANNEL    ;store received channel byte to PROCHANNEL (bank doesn't matter)
	
    incf PROSTATE	    ;wait for value byte reception by advancing PROSTATE
	
prostate1
    
    movfw PROSTATE	    ;refresh W 
    sublw 0x01		   ;sets the zero flag if PROSTATE is 1 (ie. state 1)
    btfsc STATUS,Z	    ;proceeds to the next state test if not this state 
    
    ;is there a value byte ready for reception?
    call selbank0
    btfss PIR1,RCIF	    ;if bit set indicates data is ready for read
    goto prostate1	; proceeds without processing if value byte hasn't hit yet
	
    movfw RCREG		    ;move received byte into W
    movwf PROVALUE	    ;store received channel byte to PROCHANNEL (bank doesn't matter)
	
    incf PROSTATE	    ;wait for value byte reception by advancing PROSTATE
    
prostate2
    
    movfw PROSTATE	    ;refresh W
    sublw 0x02		    ;sets the zero flag if PROSTATE is 2 (ie. state 2)
    btfsc STATUS,Z	    ;proceeds to end of state machine if not this state
    goto end_of_uart_service
    
    ;process the channel and value command!
    ; to process move PROVALUE contents to appropraite VALUE1->8/LED1->5 register
    movlw VALUE0
    addwf PROCHANNEL
    
    movwf FSR		    ;use register indirection pointed ot by FSR
    movfw PROVALUE	    ;fetch the value we want to save
    movwf 0x00		    ;Register 0x00 is a virtual indirect register
			    ;in usage here it saves PROVALUE directly to
			    ;VALUE1->8/LED1->5 register
			    
    ;that's all we have to do, the channel service routines handle the rest!
    
    clrf PROSTATE	    ;remember to set the protocol state machine to its first state
			    ;so we can receive and process more commands
    
end_of_uart_service
			    
; IMPORTANT: see if we are calling the UART service routine often enough waiting for the
; single-tasking channel service routines. Might just want to turn down the baud rate
; if we are getting overruns. Also remember a 16F88 has a 2-byte receive buffer
; so the host CAN "beam" a command over (ie. shoot over in rapid succession a 2
; byte command as it will be recevied and stored by hardware).
    
    goto start		;all this chip does is loop the algorythm ad infinitum

delay1
;we need to delay for exactly 1 millisecond
;our loop takes 2+(25*W)*200 ns to complete
;since we need a 1 ms delay, or 1000000 ns,
;W needs to be 250. 
    
    movwf CACHEW    ;cache W for restore
    
    movlw 0xFA
    movwf COUNT1
loop_d1
    decfsz COUNT1,1
    goto continue_d1	;a bit of leap frog here
    goto end_d1
continue_d1
    nop	;time wasters (to optimize put a second 20 instruction loop here)
    nop 
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop 
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    goto loop_d1
    
end_d1
    movfw CACHEW    ;remember to restore the W register on exit
    return
    
delay2
;remember we pass the channel number in W, NOT the timer value
;fetch the actual timer value by indirect addressing to VALUE1-VALUE8
;initialize the loop with the timer value and do the delay
    
;we need to delay for exactly a linear fraction of millisecond
;our loop takes 2+(25*W)*200 ns to complete
;since we need a certain delay, or 0 -> 1000000 ns,
;W needs to be 250 (or 255 it doesn't matter) for max, 0 for min. 
    movwf CACHEW    ;cache W for restore
    
    movwf FSR	    ;FSR = file select register (how you do indirect addressing)
    movfw INDF	    ;INDF = the indirectly addressed register you are accessing
		    ;the value in VALUE1 - VALUE 8 is now in W
	       
    movwf COUNT2
    
loop_d2
    decfsz COUNT2,1
    goto continue_d2	;a bit of leap frog here
    goto end_d2
continue_d2
    nop			;time wasters (to optimize put a second 
    nop			;20 instruction loop here)
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop 
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    nop
    goto loop_d2
    
end_d2

    movfw CACHEW    ;remember to restore the W register on exit
    return
    
selbank0
    
    bcf    STATUS,RP0           ;select Registers at Bank 0
    bcf    STATUS,RP1           
    return
	
selbank1
    
    bsf    STATUS,RP0           ;select Registers at Bank 1
    bcf    STATUS,RP1           
    return
	
selbank2
    
    bcf    STATUS,RP0           ;select Registers at Bank 2
    bsf    STATUS,RP1           
    return
	
selbank3
    
    bsf    STATUS,RP0           ;select Registers at Bank 3
    bsf    STATUS,RP1           
    return
    
    end

Credits

Woodward "Woody" Stanford

Woodward "Woody" Stanford

3 projects • 11 followers
Beagleboard afficianado and physics "disrupter"

Comments

Related channels and tags
  • BeagleBoard.org
  • Drones
  • GE
  • Microchip