OK, here is what some of the code looks like. Its probably pretty different than a lot of the drone code out there. A lot of it reflects the math and approaches that I used in designing my suborbitals.
#include <stdio.h>
//force-moment-mass model
struct fmm_type
{
double t1a,t1b; //motor pod 1 - in Newtons (at 100% dc)
double t1x,t1y,t1z; //in meters
double t2a,t2b; //motor pod 2 - in N (at 100% dc)
double t2x,t2y,t2z; //in m
double t3a,t3b; //motor pod 3 - in N (at 100% dc)
double t3x,t3y,t3z; //in m
double t4a,t4b; //motor pod 4 - in N (at 100% dc)
double t4x,t4y,t4z; // 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
} fmm;
//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.26.0; //voltage output of battery pack - in volts
double batt_current=0.00; //maximum current of battery pack - in amps
double batt_energy=2.56.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.t1a=0.00; //in newtons
fmm.t1b=0.00; //in N
fmm.t1x = 0.00; //in meters
fmm.t1y = 0.00; //in m
fmm.t1z = 0.00; //in m
//motor pod 2
fmm.t2a=0.00; //in newtons
fmm.t2b=0.00; //in N
fmm.t2x = 0.00; //in meters
fmm.t2y = 0.00; //in m
fmm.t2z = 0.00; //in m
//motor pod 3
fmm.t3a=0.00; //in newtons
fmm.t3b=0.00; //in N
fmm.t3x = 0.00; //in meters
fmm.t3y = 0.00; //in m
fmm.t3z = 0.00; //in m
//motor pod 4
fmm.t4a=0.00; //in newtons
fmm.t4b=0.00; //in N
fmm.t4x = 0.00; //in meters
fmm.t4y = 0.00; //in m
fmm.t4z = 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
}
main()
{
printf(“DRONED v0.89 drone flight control daemon\n”);
printf(“Initializing drone mathematical model…\n”);
initialize_fmm(); //load the fmm model into memory
printf(“Entering main loop…\n”);
while (!done)
{
//debug
done=1;
}
printf(“Daemon shutting down…returning to OS\n\n”);
return 0;
}
