diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 7740128199c7ad9c6b3ccf411a93daa80dc1aca8..59a9a923ab9a6a813e50b18be8900bcf77778952 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -3,7 +3,7 @@
#define THISFIRMWARE "ArduPlane V2.33"
/*
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler
-Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier
+Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier
Please contribute your ideas!
@@ -61,6 +61,9 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
+#include <Cellular_Modem.h> // Cellular Modem test library
+
+
////////////////////////////////////////////////////////////////////////////////
// Serial ports
////////////////////////////////////////////////////////////////////////////////
@@ -314,10 +317,10 @@ byte oldSwitchPosition;
bool inverted_flight = false;
// These are trim values used for elevon control
// For elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon
-static uint16_t elevon1_trim = 1500;
+static uint16_t elevon1_trim = 1500;
static uint16_t elevon2_trim = 1500;
// These are used in the calculation of elevon1_trim and elevon2_trim
-static uint16_t ch1_temp = 1500;
+static uint16_t ch1_temp = 1500;
static uint16_t ch2_temp = 1500;
// These are values received from the GCS if the user is using GCS joystick
// control and are substituted for the values coming from the RC radio
@@ -330,11 +333,11 @@ static bool rc_override_active = false;
////////////////////////////////////////////////////////////////////////////////
// A tracking variable for type of failsafe active
// Used for failsafe based on loss of RC signal or GCS signal
-static int failsafe;
+static int failsafe;
// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
// RC receiver should be set up to output a low throttle value when signal is lost
static bool ch3_failsafe;
-// A timer used to help recovery from unusual attitudes. If we enter an unusual attitude
+// A timer used to help recovery from unusual attitudes. If we enter an unusual attitude
// while in autonomous flight this variable is used to hold roll at 0 for a recovery period
static byte crash_timer;
// A timer used to track how long since we have received the last GCS heartbeat or rc override message
@@ -346,7 +349,7 @@ static uint32_t ch3_failsafe_timer = 0;
// LED output
////////////////////////////////////////////////////////////////////////////////
// state of the GPS light (on/off)
-static bool GPS_light;
+static bool GPS_light;
////////////////////////////////////////////////////////////////////////////////
// GPS variables
@@ -354,23 +357,23 @@ static bool GPS_light;
// This is used to scale GPS values for EEPROM storage
// 10^7 times Decimal GPS means 1 == 1cm
// This approximation makes calculations integer and it's easy to read
-static const float t7 = 10000000.0;
+static const float t7 = 10000000.0;
// We use atan2 and other trig techniques to calaculate angles
// We need to scale the longitude up to make these calcs work
// to account for decreasing distance between lines of longitude away from the equator
-static float scaleLongUp = 1;
+static float scaleLongUp = 1;
// Sometimes we need to remove the scaling for distance calcs
-static float scaleLongDown = 1;
+static float scaleLongDown = 1;
// A counter used to count down valid gps fixes to allow the gps estimate to settle
// before recording our home position (and executing a ground start if we booted with an air start)
static byte ground_start_count = 5;
-// Used to compute a speed estimate from the first valid gps fixes to decide if we are
+// Used to compute a speed estimate from the first valid gps fixes to decide if we are
// on the ground or in the air. Used to decide if a ground start is appropriate if we
// booted with an air start.
static int ground_start_avg;
// Tracks if GPS is enabled based on statup routine
-// If we do not detect GPS at startup, we stop trying and assume GPS is not connected
-static bool GPS_enabled = false;
+// If we do not detect GPS at startup, we stop trying and assume GPS is not connected
+static bool GPS_enabled = false;
////////////////////////////////////////////////////////////////////////////////
// Location & Navigation
@@ -378,29 +381,29 @@ static bool GPS_enabled = false;
// Constants
const float radius_of_earth = 6378100; // meters
const float gravity = 9.81; // meters/ sec^2
-// This is the currently calculated direction to fly.
+// This is the currently calculated direction to fly.
// deg * 100 : 0 to 360
static long nav_bearing;
-// This is the direction to the next waypoint or loiter center
+// This is the direction to the next waypoint or loiter center
// deg * 100 : 0 to 360
-static long target_bearing;
-//This is the direction from the last waypoint to the next waypoint
+static long target_bearing;
+//This is the direction from the last waypoint to the next waypoint
// deg * 100 : 0 to 360
static long crosstrack_bearing;
// A gain scaler to account for ground speed/headwind/tailwind
-static float nav_gain_scaler = 1;
+static float nav_gain_scaler = 1;
// Direction held during phases of takeoff and landing
// deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use
static long hold_course = -1; // deg * 100 dir of plane
-// There may be two active commands in Auto mode.
+// There may be two active commands in Auto mode.
// This indicates the active navigation command by index number
-static byte nav_command_index;
+static byte nav_command_index;
// This indicates the active non-navigation command by index number
-static byte non_nav_command_index;
+static byte non_nav_command_index;
// This is the command type (eg navigate to waypoint) of the active navigation command
-static byte nav_command_ID = NO_COMMAND;
-static byte non_nav_command_ID = NO_COMMAND;
+static byte nav_command_ID = NO_COMMAND;
+static byte non_nav_command_ID = NO_COMMAND;
////////////////////////////////////////////////////////////////////////////////
// Airspeed
@@ -408,21 +411,21 @@ static byte non_nav_command_ID = NO_COMMAND;
// The current airspeed estimate/measurement in centimeters per second
static int airspeed;
// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
-// Also used for flap deployment criteria. Centimeters per second.static long target_airspeed;
+// Also used for flap deployment criteria. Centimeters per second.static long target_airspeed;
static long target_airspeed;
// The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second.
-static float airspeed_error;
-// The calculated total energy error (kinetic (altitude) plus potential (airspeed)).
+static float airspeed_error;
+// The calculated total energy error (kinetic (altitude) plus potential (airspeed)).
// Used by the throttle controller
static long energy_error;
// kinetic portion of energy error (m^2/s^2)
static long airspeed_energy_error;
-// An amount that the airspeed should be increased in auto modes based on the user positioning the
+// An amount that the airspeed should be increased in auto modes based on the user positioning the
// throttle stick in the top half of the range. Centimeters per second.
static int airspeed_nudge;
// Similar to airspeed_nudge, but used when no airspeed sensor.
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
-static int throttle_nudge = 0;
+static int throttle_nudge = 0;
////////////////////////////////////////////////////////////////////////////////
// Ground speed
@@ -437,7 +440,7 @@ static long groundspeed_undershoot = 0;
static long bearing_error;
// Difference between current altitude and desired altitude. Centimeters
static long altitude_error;
-// Distance perpandicular to the course line that we are off trackline. Meters
+// Distance perpandicular to the course line that we are off trackline. Meters
static float crosstrack_error;
////////////////////////////////////////////////////////////////////////////////
@@ -448,7 +451,7 @@ static float battery_voltage1 = LOW_VOLTAGE * 1.05;
// Battery pack 1 instantaneous currrent draw. Amperes
static float current_amps1;
// Totalized current (Amp-hours) from battery 1
-static float current_total1;
+static float current_total1;
// To Do - Add support for second battery pack
//static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery 2 Voltage, initialized above threshold for filter
@@ -459,7 +462,7 @@ static float current_total1;
// Airspeed Sensors
////////////////////////////////////////////////////////////////////////////////
// Raw differential pressure measurement (filtered). ADC units
-static float airspeed_raw;
+static float airspeed_raw;
// Raw differential pressure less the zero pressure offset. ADC units
static float airspeed_pressure;
@@ -475,7 +478,7 @@ static int sonar_alt;
// flight mode specific
////////////////////////////////////////////////////////////////////////////////
// Flag for using gps ground course instead of IMU yaw. Set false when takeoff command in process.
-static bool takeoff_complete = true;
+static bool takeoff_complete = true;
// Flag to indicate if we have landed.
//Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
static bool land_complete;
@@ -492,7 +495,7 @@ static int takeoff_pitch;
// Previous target bearing. Used to calculate loiter rotations. Hundredths of a degree
static long old_target_bearing;
// Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees
-static int loiter_total;
+static int loiter_total;
// The amount in degrees we have turned since recording old_target_bearing
static int loiter_delta;
// Total rotation in a loiter. Used for Loiter Turns commands and to check for missed waypoints. Degrees
@@ -526,11 +529,11 @@ static byte event_id;
// when the event was started in ms
static long event_timer;
// how long to delay the next firing of event in millis
-static uint16_t event_delay;
+static uint16_t event_delay;
// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
static int event_repeat = 0;
// per command value, such as PWM for servos
-static int event_value;
+static int event_value;
// the value used to cycle events (alternate value to event_value)
static int event_undo_value;
@@ -563,7 +566,7 @@ static struct Location next_WP;
// The location of the active waypoint in Guided mode.
static struct Location guided_WP;
// The location structure information from the Nav command being processed
-static struct Location next_nav_command;
+static struct Location next_nav_command;
// The location structure information from the Non-Nav command being processed
static struct Location next_nonnav_command;
@@ -580,7 +583,7 @@ static long offset_altitude;
////////////////////////////////////////////////////////////////////////////////
// The main loop execution time. Seconds
//This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
-static float G_Dt = 0.02;
+static float G_Dt = 0.02;
////////////////////////////////////////////////////////////////////////////////
// Performance monitoring
@@ -611,7 +614,7 @@ static uint16_t mainLoop_count;
// Time in miliseconds of start of medium control loop. Milliseconds
static unsigned long medium_loopTimer;
// Counters for branching from main control loop to slower loops
-static byte medium_loopCounter;
+static byte medium_loopCounter;
// Number of milliseconds used in last medium loop cycle
static uint8_t delta_ms_medium_loop;
@@ -623,7 +626,7 @@ static byte superslow_loopCounter;
static byte counter_one_herz;
// used to track the elapsed time for navigation PID integral terms
-static unsigned long nav_loopTimer;
+static unsigned long nav_loopTimer;
// Elapsed time since last call to navigation pid functions
static unsigned long dTnav;
// % MCU cycles used
@@ -1143,7 +1146,7 @@ static void update_alt()
current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100)
current_loc.alt += g.altitude_mix * (read_barometer() + home.alt);
} else if (g_gps->fix) {
- current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100)
+ current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100)
}
#endif
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 3a856b827cc8a55f8fd87ed3a2a8258c6398dab8..421c45cd48ad93fc6865e78a0a29af56c5f55e92 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -127,6 +127,27 @@ static void init_ardupilot()
// used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1);
+#ifdef CELLULAR_MODEM_INIT
+
+ #ifdef CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 // we're on an APM2 board
+ // Before starting GCS on Serial, initialize the modem
+ //SendDebug("\nModem intialization on Serial0: ");
+ Cellular_Modem modem(&Serial);
+ #else
+ //SendDebug("\nModem intialization on Serial3: ");
+ Cellular_Modem modem(&Serial3);
+ #endif // APM1 or APM2
+
+ if (modem.init_modem()) {
+ //SendDebug("success.\n");
+ }
+ else {
+ //SendDebug("failed.\n");
+ }
+
+#endif // CELLULAR_MODEM_INIT
+
+
// init the GCS
gcs0.init(&Serial);