Added Headfree mode

project files/bradwii.cpp

@@ -161,6 +161,8 @@ int main(void)
                #if (GPS_TYPE!=NO_GPS)
                   navigation_sethometocurrentlocation();
                #endif
+               global.heading_when_armed=global.currentestimatedeulerattitude[YAWINDEX];
+               global.altitude_when_armed=global.barorawaltitude;
                }
             }
          else if (!(global.activecheckboxitems & CHECKBOXMASKARM)) global.armed=0;
@@ -400,6 +402,12 @@ int main(void)
       // calculate pid outputs based on our angleerrors as inputs
       fixedpointnum pidoutput[3];
 
+      // Gain Scheduling essentialy modifies the gains depending on
+      // throttle level. If GAIN_SCHEDULING_FACTOR is 1.0, it multiplies PID outputs by 1.5 when at full throttle,
+      // 1.0 when at mid throttle, and .5 when at zero throttle.  This helps
+      // eliminate the wobbles when decending at low throttle.
+      fixedpointnum gainschedulingmultiplier=lib_fp_multiply(throttleoutput-FIXEDPOINTCONSTANT(.5),FIXEDPOINTCONSTANT(GAIN_SCHEDULING_FACTOR))+FIXEDPOINTONE;
+
       for (int x=0;x<3;++x)
          {
          integratedangleerror[x]+=lib_fp_multiply(angleerror[x],global.timesliver);
@@ -412,13 +420,8 @@ int main(void)
             -lib_fp_multiply(global.gyrorate[x],usersettings.pid_dgain[x])
             +(lib_fp_multiply(integratedangleerror[x],usersettings.pid_igain[x])>>4);
 
-         // add gain scheduling.  Essentialy modifies the gains depending on
-         // throttle level. Multiplies PID outputs by 1.5 when at full throttle,
-         // 1.0 when at mid throttle, and .5 when at zero throttle.  This helps
-         // eliminate the wobbles when decending at low throttle.
-#ifndef NOGAINSCHEDULING
-         pidoutput[x]=lib_fp_multiply(throttleoutput+FIXEDPOINTCONSTANT(.5),pidoutput[x]);
-#endif
+         // add gain scheduling.  
+         pidoutput[x]=lib_fp_multiply(gainschedulingmultiplier,pidoutput[x]);
          }
 
       lib_fp_constrain(&throttleoutput,0,FIXEDPOINTONE);

project files/bradwii.h

@@ -62,15 +62,15 @@ typedef struct
    fixedpointnum currentestimatedeulerattitude[3]; // Euler Angles in degrees of how much we think the aircraft is Rolled, Pitched, and Yawed (from North)
    fixedpointnum rxvalues[RXNUMCHANNELS];            // The values of the RX inputs, ranging from -1.0 to 1.0
    fixedpointnum compassvector[3];                  // A unit vector (approximately) poining in the direction our 3d compass is pointing
+   fixedpointnum heading_when_armed;                  // the heading we were pointing when arming was established
+   fixedpointnum altitude_when_armed;                  // The altitude when arming established
    unsigned int motoroutputvalue[NUMMOTORS];         // Output values to send to our motors, from 1000 to 2000
    unsigned long activecheckboxitems;               // Bits for each checkbox item to show which are currently active
    unsigned long previousactivecheckboxitems;      // The previous state of these bits so we can tell when they turn on and off
    unsigned char armed;                              // A flag indicating that the aircraft is armed
    unsigned char gps_num_satelites;                  // How many satelites do we currently see?
    fixedpointnum gps_home_latitude;                  // The latitude when the aircraft was armed
    fixedpointnum gps_home_longitude;               // The longitude when the aircraft was armed
-   fixedpointnum gps_home_heading;                  // the heading we were pointing when home was established
-   fixedpointnum gps_home_altitude;                  // The altitude when home was established
    fixedpointnum gps_current_latitude;               // The current GPS latitude
    fixedpointnum gps_current_longitude;            // The current GPS longitude
    fixedpointnum gps_current_altitude;               // The current GPS altitude

project files/checkboxes.cpp

@@ -35,7 +35,8 @@ char checkboxnames[]/* PROGMEM */= // names for dynamic generation of config GUI
    "High Rates;"
    "High Angle;"
    "Auto Tune;"
-   "Uncrashable"
+   "Uncrashable;"
+   "HF"
    ;
 
 // each checkbox item has a checkboxvalue.  The bits in this value represent low, medium, and high checkboxes

project files/checkboxes.h

@@ -29,7 +29,8 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define CHECKBOXHIGHANGLE 9
 #define CHECKBOXAUTOTUNE 10
 #define CHECKBOXUNCRASHABLE 11
-#define NUMCHECKBOXES 12
+#define CHECKBOXHEADFREE 12
+#define NUMCHECKBOXES 13
 
 #define CHECKBOXMASKARM (1<<CHECKBOXARM)
 #define CHECKBOXMASKAUTOTHROTTLE (1<<CHECKBOXAUTOTHROTTLE)
@@ -43,6 +44,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define CHECKBOXMASKHIGHANGLE (1<<CHECKBOXHIGHANGLE)
 #define CHECKBOXMASKAUTOTUNE (1<<CHECKBOXAUTOTUNE)
 #define CHECKBOXMASKUNCRASHABLE (1<<CHECKBOXUNCRASHABLE)
+#define CHECKBOXMASKHEADFREE (1<<CHECKBOXHEADFREE)
 
 
 #define CHECKBOXMASKAUX1LOW (1<<0)

project files/config.h

@@ -31,10 +31,10 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //#define CONTROL_BOARD_TYPE CONTROL_BOARD_SIRIUS_AIR_GPS
 
 // Choose the type of r/c reciever that will be used
-//#define RX_TYPE RX_NORMAL
+#define RX_TYPE RX_NORMAL
 //#define RX_TYPE RX_CPPM
 //#define RX_TYPE RX_DSM2_1024
-#define RX_TYPE RX_DSM2_2048
+//#define RX_TYPE RX_DSM2_2048
 //#define RX_DSM2_SERIAL_PORT 1
 
 // Choose a channel order if you don't like the default for your receiver type selected above
@@ -94,7 +94,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define MIN_MOTOR_OUTPUT 1000
 #define MAX_MOTOR_OUTPUT 2000
 #define ARMED_MIN_MOTOR_OUTPUT 1067 // motors spin slowly when armed
-#define FAILSAFE_MOTOR_OUTPUT 1200 // throttle setting for bringing the aircraft down at a safe speed
+#define FAILSAFE_MOTOR_OUTPUT 1100 // throttle setting for bringing the aircraft down at a safe speed
 
 // Divide the Aux inputs into low, medium, and high using the following divisions
 #define AUX_MID_RANGE_LOW 1300
@@ -117,9 +117,9 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 // To calibrate all ESCs connected to the aircraft at the same time (useful to avoid unplugging/re-plugging each ESC)
 // Warning: this creates a special version of code
 //       You cannot fly with this special version. It is only to be used for calibrating ESCs
-// #define ESC_CALIB_CANNOT_FLY  // uncomment to activate
- #define ESC_CALIB_LOW  MIN_MOTOR_OUTPUT
- #define ESC_CALIB_HIGH MAX_MOTOR_OUTPUT
+//#define ESC_CALIB_CANNOT_FLY  // uncomment to activate
+#define ESC_CALIB_LOW  MIN_MOTOR_OUTPUT
+#define ESC_CALIB_HIGH MAX_MOTOR_OUTPUT
 
 // un-comment if you don't want to include autotune code
 //#define NO_AUTOTUNE
@@ -141,4 +141,4 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 // Uncomment the following line if you want to turn off gain scheduling.  Gain scheduling adjusts the PID gains
 // depending on the level of throttle.  It attempts to eliminate the wobbles while decending under low throttle.
-// #define NOGAINSCHEDULING
+#define GAIN_SCHEDULING_FACTOR 1.0

project files/defs.h

@@ -236,7 +236,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
    #define MOTOR_4_CHANNEL (OUTPUT_TIMER1 | OUTPUT_CHANNELC)
    #define MOTOR_4_PIN (DIGITALPORTB | 7)
    #define MOTOR_5_CHANNEL (OUTPUT_TIMER4 | OUTPUT_CHANNELA)
-   #define MOTOR_5_PIN (DIGITALPORTC7 | 1)
+   #define MOTOR_5_PIN (DIGITALPORTC | 1)
 
 #endif
 

project files/gps.cpp

@@ -62,7 +62,8 @@ unsigned char gpsframetype;
 unsigned char gpschecksum;
 unsigned char gpsfinalchecksum;
 unsigned char gpsfix;
-fixedpointnum gpsdegrees;
+fixedpointnum gpslatitudedegrees;
+fixedpointnum gpslongitudedegrees;
 
 unsigned char hextochar(unsigned char n) 
    {      // convert '0'..'9','A'..'F' to 0..15
@@ -113,21 +114,19 @@ char readgps()
                {
                if (gpsparameternumber == 2) 
                   {
-                  gpsdegrees = gpsstringtoangle(gpsdata);
+                  gpslatitudedegrees = gpsstringtoangle(gpsdata);
                   }
-               else if (gpsparameternumber == 3 && gpsdegrees!=0)
+               else if (gpsparameternumber == 3)
                   {
-                  if ( gpsdata[0] == 'S') global.gps_current_latitude=-gpsdegrees;
-                  else global.gps_current_latitude=gpsdegrees;
+                  if ( gpsdata[0] == 'S') gpslatitudedegrees=-gpslatitudedegrees;
                   }
                else if (gpsparameternumber == 4)
                   {
-                  gpsdegrees = gpsstringtoangle(gpsdata);
+                  gpslongitudedegrees = gpsstringtoangle(gpsdata);
                   }
-               else if (gpsparameternumber == 5 && gpsdegrees!=0)
+               else if (gpsparameternumber == 5)
                   {
-                  if ( gpsdata[0] == 'W') global.gps_current_longitude=-gpsdegrees;
-                  else global.gps_current_longitude=gpsdegrees;
+                  if ( gpsdata[0] == 'W') gpslongitudedegrees=-gpslongitudedegrees;
                   }
                else if (gpsparameternumber == 6)
                   {
@@ -168,6 +167,9 @@ char readgps()
             if (checksum != gpsfinalchecksum || gpsframetype!=FRAME_GGA || !gpsfix) return(0);
             gpsframetype=0; // so we don't check again
 
+            global.gps_current_latitude=gpslatitudedegrees;
+            global.gps_current_longitude=gpslongitudedegrees;
+
             return(1); // we got a good frame
             }
          else if (gpsdataindex<GPSDATASIZE)

project files/navigation.cpp

@@ -86,8 +86,6 @@ void navigation_sethometocurrentlocation()
    {
    global.gps_home_latitude=global.gps_current_latitude;
    global.gps_home_longitude=global.gps_current_longitude;
-   global.gps_home_heading=global.currentestimatedeulerattitude[YAWINDEX];
-   global.gps_home_altitude=global.barorawaltitude;
    }
 
 void navigation_set_destination(fixedpointnum latitude,fixedpointnum longitude)

project files/output.cpp

@@ -41,6 +41,9 @@ void initoutputs()
 #ifdef USEPWM411BIT
    lib_pwm_init4(PWM411BITPHASECORRECTMODE,/*PWM411BITNORMALOUTPUTA | PWM411BITNORMALOUTPUTB |*/ PWM411BITNORMALOUTPUTD,PWM411BITPRESCALER16,0x7FF); // TOP to 2047
 #endif
+#ifdef USEPWM5
+   lib_pwm_init5(PWM5PHASECORRECTMODE,PWM5NORMALOUTPUTA | PWM5NORMALOUTPUTB | PWM5NORMALOUTPUTC,PWM5PRESCALER1,0x3FFF); // TOP to 16383
+#endif
 
    lib_digitalio_initpin(MOTOR_0_PIN, DIGITALOUTPUT);
    lib_digitalio_initpin(MOTOR_1_PIN, DIGITALOUTPUT);
@@ -58,13 +61,14 @@ void initoutputs()
  /********  special version of MultiWii to calibrate all attached ESCs ************/
 #if defined(ESC_CALIB_CANNOT_FLY)
     setallmotoroutputs(ESC_CALIB_HIGH);
-       lib_timers_delaymilliseconds(3000);
+    lib_timers_delaymilliseconds(3000);
     setallmotoroutputs(ESC_CALIB_LOW);
-    while (1) 
+
+    while (1)
       {
-         lib_timers_delaymilliseconds(500);
+      lib_timers_delaymilliseconds(500);
       lib_digitalio_setoutput(LED1_OUTPUT, 0);
-         lib_timers_delaymilliseconds(500);
+      lib_timers_delaymilliseconds(500);
       lib_digitalio_setoutput(LED1_OUTPUT, 1);
       }
 #endif

project files/pilotcontrol.cpp

@@ -50,6 +50,24 @@ void resetpilotcontrol()
 void getangleerrorfrompilotinput(fixedpointnum *angleerror)
    {
    // sets the ange errors for roll, pitch, and yaw based on where the pilot has the tx sticks.
+   fixedpointnum rxrollvalue;
+   fixedpointnum rxpitchvalue;
+
+   // if in headfree mode, rotate the pilot's stick inputs by the angle that is the difference between where we are currently heading and where we were heading when we armed.
+   if (global.activecheckboxitems & CHECKBOXMASKHEADFREE)
+      {
+      fixedpointnum angledifference = global.currentestimatedeulerattitude[YAWINDEX] - global.heading_when_armed;
+
+      fixedpointnum cosangledifference = lib_fp_cosine(angledifference);
+      fixedpointnum sinangledifference = lib_fp_sine(angledifference);
+      rxpitchvalue = lib_fp_multiply(global.rxvalues[PITCHINDEX],cosangledifference) + lib_fp_multiply(global.rxvalues[ROLLINDEX],sinangledifference);
+      rxrollvalue =  lib_fp_multiply(global.rxvalues[ROLLINDEX],cosangledifference) - lib_fp_multiply(global.rxvalues[PITCHINDEX],sinangledifference);
+      }
+   else
+      {
+      rxpitchvalue=global.rxvalues[PITCHINDEX];
+      rxrollvalue=global.rxvalues[ROLLINDEX];
+      }
 
    // first, calculate level mode values
    // how far is our estimated current attitude from our desired attitude?
@@ -60,8 +78,8 @@ void getangleerrorfrompilotinput(fixedpointnum *angleerror)
    else levelmodemaxangle=FP_LEVEL_MODE_MAX_TILT;
 
    // the angle error is how much our current angles differ from our desired angles.
-   fixedpointnum levelmoderollangleerror=lib_fp_multiply(global.rxvalues[ROLLINDEX],levelmodemaxangle)-global.currentestimatedeulerattitude[ROLLINDEX];
-   fixedpointnum levelmodepitchangleerror=lib_fp_multiply(global.rxvalues[PITCHINDEX],levelmodemaxangle)-global.currentestimatedeulerattitude[PITCHINDEX];
+   fixedpointnum levelmoderollangleerror=lib_fp_multiply(rxrollvalue,levelmodemaxangle)-global.currentestimatedeulerattitude[ROLLINDEX];
+   fixedpointnum levelmodepitchangleerror=lib_fp_multiply(rxpitchvalue,levelmodemaxangle)-global.currentestimatedeulerattitude[PITCHINDEX];
 
    // In acro mode, we want the rotation rate to be proportional to the pilot's stick movement.  The desired rotation rate is
    // the stick movement * a multiplier.
@@ -82,8 +100,8 @@ void getangleerrorfrompilotinput(fixedpointnum *angleerror)
       maxpitchandrollrate=usersettings.maxpitchandrollrate;
       }
 
-   angleerror[ROLLINDEX]=lib_fp_multiply(lib_fp_multiply(global.rxvalues[ROLLINDEX],maxpitchandrollrate)-global.gyrorate[ROLLINDEX],global.timesliver);
-   angleerror[PITCHINDEX]=lib_fp_multiply(lib_fp_multiply(global.rxvalues[PITCHINDEX],maxpitchandrollrate)-global.gyrorate[PITCHINDEX],global.timesliver);
+   angleerror[ROLLINDEX]=lib_fp_multiply(lib_fp_multiply(rxrollvalue,maxpitchandrollrate)-global.gyrorate[ROLLINDEX],global.timesliver);
+   angleerror[PITCHINDEX]=lib_fp_multiply(lib_fp_multiply(rxpitchvalue,maxpitchandrollrate)-global.gyrorate[PITCHINDEX],global.timesliver);
 
    // put a low pass filter on the yaw gyro.  If we don't do this, things can get jittery.
    lib_fp_lowpassfilter(&filteredyawgyrorate, global.gyrorate[YAWINDEX], global.timesliver>>(TIMESLIVEREXTRASHIFT-3), FIXEDPOINTONEOVERONESIXTYITH, 3);
@@ -130,17 +148,17 @@ void getangleerrorfrompilotinput(fixedpointnum *angleerror)
       // figure out how much the most moved stick is from centered
       fixedpointnum maxstickthrow;
 
-      if (global.rxvalues[ROLLINDEX]<0)
-         maxstickthrow=-global.rxvalues[ROLLINDEX];
+      if (rxrollvalue<0)
+         maxstickthrow=-rxrollvalue;
       else
-         maxstickthrow=global.rxvalues[ROLLINDEX];
+         maxstickthrow=rxrollvalue;
 
-      if (global.rxvalues[PITCHINDEX]<0)
+      if (rxpitchvalue<0)
          {
-         if (-global.rxvalues[PITCHINDEX]>maxstickthrow) maxstickthrow=-global.rxvalues[PITCHINDEX];
+         if (-rxpitchvalue>maxstickthrow) maxstickthrow=-rxpitchvalue;
          }
       else
-         if (global.rxvalues[PITCHINDEX]>maxstickthrow) maxstickthrow=global.rxvalues[PITCHINDEX];
+         if (rxpitchvalue>maxstickthrow) maxstickthrow=rxpitchvalue;
 
 
       // if the aircraft is tipped more than 90 degrees, use full acro mode so we don't run into

project files/serial.cpp

@@ -36,7 +36,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "gps.h"
 
 #define MSP_VERSION 0
-#define  VERSION  103 // version 1.03
+#define  VERSION  105 // version 1.05
 
 
 extern globalstruct global;