Things used in this project

Hardware components:
Fcz8v3vekvexkiar05ne BeagleBone Black Wireless
NEO-6M GPS Module
GY-521 MPU-6050 3 Axis Gyroscope + Accelerometer Module For Arduino
GE Hobby ESC - SimonK Firmware - 30 Amp
Microchip PIC16F
3000KV 3-Phase Drone Motor
Software apps and online services:
GNU C/C++ (ANSI/POSIX-Compliant-Shell Access)
Hand tools and fabrication machines:
GE Fiscars Drill


Drone Block Diagram 1
Semi-completed drone
Ssdrone1 zwe8wxqrjd


Sample FMM modelC/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

	//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

	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; //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.ay=0.0;; //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)


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;	

	//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},

	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++)
	//load flight plan from loading array, fp1
	for (a=0;a<fplan.num_waypoints;a++)

	//DEBUG: save a copy of flight plan to filesystem

	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



	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);


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)

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

	//make motor force vectors

	//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

	//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 * timeslice; * timeslice;

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

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

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

	//total force right

	//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 += daf;
	curdrone.ay += dar; += dau;
	//calculate velocity from acceleration * timeslice; //timeslice is necessary for proper integration
	curdrone.vy+=curdrone.ay * timeslice; * 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...

setmotors(double dutycycle)



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

	printf("Initializing graph3 graphics support...");
	textout(0,50,20,"Drone Flight Simulation",2);

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

/* 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...");

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

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

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

			case OFF: //kill motors


				if (go_signal) curstate=ON;


			case ON: //bring motors to low idle


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


			case ASCEND: //take off vertically to initial altitude

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


			case AQUIRE: //precess so facing next waypoint


			case TRAVEL: //continue to next way point

			case NEUTRAL: //stop in the air


			case DESCEND: //land vertically to zero altitude


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



		//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
		bar2(0,curdrone.x*5+(640/2), (480/2)-curdrone.y*5,1,1);



	printf("Writting graphical representation of flight to flight.bmp...");

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



Woody1 nwvucw1sa6
Woody Stanford

Beagleboard afficianado and physics "disrupter"



Did you replicate this project? Share it!

I made one

Love this project? Think it could be improved? Tell us what you think!

Give feedback


Similar projects you might like

Velo Bling-Bling
  • 7,865
  • 153

Full instructions

LED lights for bicycle wheels. 16 multi color LEDs on both PCB sides. Displays images and dynamic info like speed. BLE and USB interface.

Arduino PICO: The World’s Smallest Arduino Board
  • 357
  • 6

Meet the world’s smallest Arduino board with 0.6”x0.6” dimensions ready to rock!

Solar Power Module V2
  • 2,054
  • 26

Full instructions

Single board to control solar charge and battery monitoring for your outdoor projects. Your project will see a perpetually charged battery.

My motorbike Telemetry
  • 12,914
  • 58

Full instructions

Telemetry system of my Ducati Monster based on Arduino, UWP, Azure features and Machine Learning. Enterprise IoT Cloud Solution.

Raspberry Pi Wireless Home Automation With Azure DB Support
  • 10,379
  • 61

Full instructions

Control your home appliances from anywhere in the world with automation options.

Evive: a Prototyping Platform for Makers
  • 3,324
  • 27

An open-source Arduino based toolkit to learn, build & debug electronics and robotics projects.

Sign up / LoginProjectsPlatformsTopicsContestsLiveAppsBetaFree StoreBlog