#include
#include
#include //required for graph3
#include
//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;ax=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;
}