Plane: convert from .pde to .cpp files

ArduPlane/ArduPlane.cpp

@@ -0,0 +1,934 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+   Lead developer: Andrew Tridgell
+ 
+   Authors:    Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher, Paul Riseborough, Brandon Jones, Jon Challinger
+   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, Yury MonZon
+
+   Please contribute your ideas! See http://dev.ardupilot.com for details
+
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "Plane.h"
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpmf-conversions"
+#define SCHED_TASK(func) AP_HAL_CLASSPROC_VOID(&plane, &Plane::func)
+
+/*
+  scheduler table - all regular tasks are listed here, along with how
+  often they should be called (in 20ms units) and the maximum time
+  they are expected to take (in microseconds)
+ */
+const AP_Scheduler::Task Plane::scheduler_tasks[] PROGMEM = {
+    { SCHED_TASK(read_radio),             1,    700 }, // 0
+    { SCHED_TASK(check_short_failsafe),   1,   1000 },
+    { SCHED_TASK(ahrs_update),            1,   6400 },
+    { SCHED_TASK(update_speed_height),    1,   1600 },
+    { SCHED_TASK(update_flight_mode),     1,   1400 },
+    { SCHED_TASK(stabilize),              1,   3500 },
+    { SCHED_TASK(set_servos),             1,   1600 },
+    { SCHED_TASK(read_control_switch),    7,   1000 },
+    { SCHED_TASK(gcs_retry_deferred),     1,   1000 },
+    { SCHED_TASK(update_GPS_50Hz),        1,   2500 },
+    { SCHED_TASK(update_GPS_10Hz),        5,   2500 }, // 10
+    { SCHED_TASK(navigate),               5,   3000 },
+    { SCHED_TASK(update_compass),         5,   1200 },
+    { SCHED_TASK(read_airspeed),          5,   1200 },
+    { SCHED_TASK(update_alt),             5,   3400 },
+    { SCHED_TASK(adjust_altitude_target), 5,   1000 },
+    { SCHED_TASK(obc_fs_check),           5,   1000 },
+    { SCHED_TASK(gcs_update),             1,   1700 },
+    { SCHED_TASK(gcs_data_stream_send),   1,   3000 },
+    { SCHED_TASK(update_events),		  1,   1500 }, // 20
+    { SCHED_TASK(check_usb_mux),          5,    300 },
+    { SCHED_TASK(read_battery),           5,   1000 },
+    { SCHED_TASK(compass_accumulate),     1,   1500 },
+    { SCHED_TASK(barometer_accumulate),   1,    900 },
+    { SCHED_TASK(update_notify),          1,    300 },
+    { SCHED_TASK(read_rangefinder),       1,    500 },
+#if OPTFLOW == ENABLED
+    { SCHED_TASK(update_optical_flow),    1,    500 },
+#endif
+    { SCHED_TASK(one_second_loop),       50,   1000 },
+    { SCHED_TASK(check_long_failsafe),   15,   1000 },
+    { SCHED_TASK(read_receiver_rssi),     5,   1000 },
+    { SCHED_TASK(airspeed_ratio_update), 50,   1000 }, // 30
+    { SCHED_TASK(update_mount),           1,   1500 },
+    { SCHED_TASK(log_perf_info),        500,   1000 },
+    { SCHED_TASK(compass_save),        3000,   2500 },
+    { SCHED_TASK(update_logging1),        5,   1700 },
+    { SCHED_TASK(update_logging2),        5,   1700 },
+#if FRSKY_TELEM_ENABLED == ENABLED
+    { SCHED_TASK(frsky_telemetry_send),  10,    100 },
+#endif
+    { SCHED_TASK(terrain_update),         5,    500 },
+};
+
+#pragma GCC diagnostic pop
+
+void Plane::setup() 
+{
+    cliSerial = hal.console;
+
+    // load the default values of variables listed in var_info[]
+    AP_Param::setup_sketch_defaults();
+
+    AP_Notify::flags.failsafe_battery = false;
+
+    notify.init(false);
+
+    rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
+
+    init_ardupilot();
+
+    // initialise the main loop scheduler
+    scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]), NULL);
+}
+
+void Plane::loop()
+{
+    // wait for an INS sample
+    ins.wait_for_sample();
+
+    uint32_t timer = hal.scheduler->micros();
+
+    delta_us_fast_loop  = timer - fast_loopTimer_us;
+    G_Dt                = delta_us_fast_loop * 1.0e-6f;
+
+    if (delta_us_fast_loop > G_Dt_max && fast_loopTimer_us != 0) {
+        G_Dt_max = delta_us_fast_loop;
+    }
+
+    if (delta_us_fast_loop < G_Dt_min || G_Dt_min == 0) {
+        G_Dt_min = delta_us_fast_loop;
+    }
+    fast_loopTimer_us   = timer;
+
+    mainLoop_count++;
+
+    // tell the scheduler one tick has passed
+    scheduler.tick();
+
+    // run all the tasks that are due to run. Note that we only
+    // have to call this once per loop, as the tasks are scheduled
+    // in multiples of the main loop tick. So if they don't run on
+    // the first call to the scheduler they won't run on a later
+    // call until scheduler.tick() is called again
+    uint32_t remaining = (timer + 20000) - hal.scheduler->micros();
+    if (remaining > 19500) {
+        remaining = 19500;
+    }
+    scheduler.run(remaining);
+}
+
+// update AHRS system
+void Plane::ahrs_update()
+{
+    hal.util->set_soft_armed(arming.is_armed() &&
+                   hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
+
+    if (g.hil_mode == 1) {
+        // update hil before AHRS update
+        gcs_update();
+    }
+
+    ahrs.update();
+
+    if (should_log(MASK_LOG_ATTITUDE_FAST)) {
+        Log_Write_Attitude();
+    }
+
+    if (should_log(MASK_LOG_IMU))
+        Log_Write_IMU();
+
+    // calculate a scaled roll limit based on current pitch
+    roll_limit_cd = g.roll_limit_cd * cosf(ahrs.pitch);
+    pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll));
+
+    // updated the summed gyro used for ground steering and
+    // auto-takeoff. Dot product of DCM.c with gyro vector gives earth
+    // frame yaw rate
+    steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
+    steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
+}
+
+/*
+  update 50Hz speed/height controller
+ */
+void Plane::update_speed_height(void)
+{
+    if (auto_throttle_mode) {
+	    // Call TECS 50Hz update. Note that we call this regardless of
+	    // throttle suppressed, as this needs to be running for
+	    // takeoff detection
+        SpdHgt_Controller->update_50hz(tecs_hgt_afe());
+    }
+}
+
+
+/*
+  update camera mount
+ */
+void Plane::update_mount(void)
+{
+#if MOUNT == ENABLED
+    camera_mount.update();
+#endif
+
+#if CAMERA == ENABLED
+    camera.trigger_pic_cleanup();
+#endif
+}
+
+/*
+  read and update compass
+ */
+void Plane::update_compass(void)
+{
+    if (g.compass_enabled && compass.read()) {
+        ahrs.set_compass(&compass);
+        compass.learn_offsets();
+        if (should_log(MASK_LOG_COMPASS)) {
+            DataFlash.Log_Write_Compass(compass);
+        }
+    } else {
+        ahrs.set_compass(NULL);
+    }
+}
+
+/*
+  if the compass is enabled then try to accumulate a reading
+ */
+void Plane::compass_accumulate(void)
+{
+    if (g.compass_enabled) {
+        compass.accumulate();
+    }    
+}
+
+/*
+  try to accumulate a baro reading
+ */
+void Plane::barometer_accumulate(void)
+{
+    barometer.accumulate();
+}
+
+/*
+  do 10Hz logging
+ */
+void Plane::update_logging1(void)
+{
+    if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
+        Log_Write_Attitude();
+    }
+
+    if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU))
+        Log_Write_IMU();
+}
+
+/*
+  do 10Hz logging - part2
+ */
+void Plane::update_logging2(void)
+{
+    if (should_log(MASK_LOG_CTUN))
+        Log_Write_Control_Tuning();
+    
+    if (should_log(MASK_LOG_NTUN))
+        Log_Write_Nav_Tuning();
+
+    if (should_log(MASK_LOG_RC))
+        Log_Write_RC();
+}
+
+
+/*
+  check for OBC failsafe check
+ */
+void Plane::obc_fs_check(void)
+{
+#if OBC_FAILSAFE == ENABLED
+    // perform OBC failsafe checks
+    obc.check(OBC_MODE(control_mode), failsafe.last_heartbeat_ms, geofence_breached(), failsafe.last_valid_rc_ms);
+#endif
+}
+
+
+/*
+  update aux servo mappings
+ */
+void Plane::update_aux(void)
+{
+    if (!px4io_override_enabled) {
+        RC_Channel_aux::enable_aux_servos();
+    }
+}
+
+void Plane::one_second_loop()
+{
+    if (should_log(MASK_LOG_CURRENT))
+        Log_Write_Current();
+
+    // send a heartbeat
+    gcs_send_message(MSG_HEARTBEAT);
+
+    // make it possible to change control channel ordering at runtime
+    set_control_channels();
+
+    // make it possible to change orientation at runtime
+    ahrs.set_orientation();
+
+    // sync MAVLink system ID
+    mavlink_system.sysid = g.sysid_this_mav;
+
+    update_aux();
+
+    // determine if we are flying or not
+    determine_is_flying();
+
+    // update notify flags
+    AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
+    AP_Notify::flags.pre_arm_gps_check = true;
+    AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
+
+#if AP_TERRAIN_AVAILABLE
+    if (should_log(MASK_LOG_GPS)) {
+        terrain.log_terrain_data(DataFlash);
+    }
+#endif
+    // piggyback the status log entry on the MODE log entry flag
+    if (should_log(MASK_LOG_MODE)) {
+        Log_Write_Status();
+    }
+
+    ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
+}
+
+void Plane::log_perf_info()
+{
+    if (scheduler.debug() != 0) {
+        gcs_send_text_fmt(PSTR("G_Dt_max=%lu G_Dt_min=%lu\n"), 
+                          (unsigned long)G_Dt_max, 
+                          (unsigned long)G_Dt_min);
+    }
+    if (should_log(MASK_LOG_PM))
+        Log_Write_Performance();
+    G_Dt_max = 0;
+    G_Dt_min = 0;
+    resetPerfData();
+}
+
+void Plane::compass_save()
+{
+    if (g.compass_enabled) {
+        compass.save_offsets();
+    }
+}
+
+void Plane::terrain_update(void)
+{
+#if AP_TERRAIN_AVAILABLE
+    terrain.update();
+
+    // tell the rangefinder our height, so it can go into power saving
+    // mode if available
+    float height;
+    if (terrain.height_above_terrain(height, true)) {
+        rangefinder.set_estimated_terrain_height(height);
+    }
+#endif
+}
+
+/*
+  once a second update the airspeed calibration ratio
+ */
+void Plane::airspeed_ratio_update(void)
+{
+    if (!airspeed.enabled() ||
+        gps.status() < AP_GPS::GPS_OK_FIX_3D ||
+        gps.ground_speed() < 4) {
+        // don't calibrate when not moving
+        return;        
+    }
+    if (airspeed.get_airspeed() < aparm.airspeed_min && 
+        gps.ground_speed() < (uint32_t)aparm.airspeed_min) {
+        // don't calibrate when flying below the minimum airspeed. We
+        // check both airspeed and ground speed to catch cases where
+        // the airspeed ratio is way too low, which could lead to it
+        // never coming up again
+        return;
+    }
+    if (abs(ahrs.roll_sensor) > roll_limit_cd ||
+        ahrs.pitch_sensor > aparm.pitch_limit_max_cd ||
+        ahrs.pitch_sensor < pitch_limit_min_cd) {
+        // don't calibrate when going beyond normal flight envelope
+        return;
+    }
+    const Vector3f &vg = gps.velocity();
+    airspeed.update_calibration(vg);
+    gcs_send_airspeed_calibration(vg);
+}
+
+
+/*
+  read the GPS and update position
+ */
+void Plane::update_GPS_50Hz(void)
+{
+    static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
+    gps.update();
+
+    for (uint8_t i=0; i<gps.num_sensors(); i++) {
+        if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
+            last_gps_reading[i] = gps.last_message_time_ms(i);
+            if (should_log(MASK_LOG_GPS)) {
+                Log_Write_GPS(i);
+            }
+        }
+    }
+}
+
+/*
+  read update GPS position - 10Hz update
+ */
+void Plane::update_GPS_10Hz(void)
+{
+    // get position from AHRS
+    have_position = ahrs.get_position(current_loc);
+
+    static uint32_t last_gps_msg_ms;
+    if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        last_gps_msg_ms = gps.last_message_time_ms();
+
+        if (ground_start_count > 1) {
+            ground_start_count--;
+        } else if (ground_start_count == 1) {
+            // We countdown N number of good GPS fixes
+            // so that the altitude is more accurate
+            // -------------------------------------
+            if (current_loc.lat == 0) {
+                ground_start_count = 5;
+
+            } else {
+                init_home();
+
+                // set system clock for log timestamps
+                hal.util->set_system_clock(gps.time_epoch_usec());
+
+                if (g.compass_enabled) {
+                    // Set compass declination automatically
+                    const Location &loc = gps.location();
+                    compass.set_initial_location(loc.lat, loc.lng);
+                }
+                ground_start_count = 0;
+            }
+        }
+
+        // see if we've breached the geo-fence
+        geofence_check(false);
+
+#if CAMERA == ENABLED
+        if (camera.update_location(current_loc) == true) {
+            do_take_picture();
+        }
+#endif        
+
+        if (!hal.util->get_soft_armed()) {
+            update_home();
+        }
+
+        // update wind estimate
+        ahrs.estimate_wind();
+    }
+
+    calc_gndspeed_undershoot();
+}
+
+/*
+  main handling for AUTO mode
+ */
+void Plane::handle_auto_mode(void)
+{
+    uint8_t nav_cmd_id;
+
+    // we should be either running a mission or RTLing home
+    if (mission.state() == AP_Mission::MISSION_RUNNING) {
+        nav_cmd_id = mission.get_current_nav_cmd().id;
+    }else{
+        nav_cmd_id = auto_rtl_command.id;
+    }
+
+    switch(nav_cmd_id) {
+    case MAV_CMD_NAV_TAKEOFF:
+        takeoff_calc_roll();
+        takeoff_calc_pitch();
+        calc_throttle();
+
+        break;
+
+    case MAV_CMD_NAV_LAND:
+        calc_nav_roll();
+        calc_nav_pitch();
+        
+        if (auto_state.land_complete) {
+            // during final approach constrain roll to the range
+            // allowed for level flight
+            nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
+        }
+        calc_throttle();
+        
+        if (auto_state.land_complete) {
+            // we are in the final stage of a landing - force
+            // zero throttle
+            channel_throttle->servo_out = 0;
+        }
+        break;
+        
+    default:
+        // we are doing normal AUTO flight, the special cases
+        // are for takeoff and landing
+        steer_state.hold_course_cd = -1;
+        auto_state.land_complete = false;
+        auto_state.land_sink_rate = 0;
+        calc_nav_roll();
+        calc_nav_pitch();
+        calc_throttle();
+        break;
+    }
+}
+
+/*
+  main flight mode dependent update code 
+ */
+void Plane::update_flight_mode(void)
+{
+    enum FlightMode effective_mode = control_mode;
+    if (control_mode == AUTO && g.auto_fbw_steer) {
+        effective_mode = FLY_BY_WIRE_A;
+    }
+
+    if (effective_mode != AUTO) {
+        // hold_course is only used in takeoff and landing
+        steer_state.hold_course_cd = -1;
+    }
+
+    switch (effective_mode) 
+    {
+    case AUTO:
+        handle_auto_mode();
+        break;
+
+    case RTL:
+    case LOITER:
+    case GUIDED:
+        calc_nav_roll();
+        calc_nav_pitch();
+        calc_throttle();
+        break;
+        
+    case TRAINING: {
+        training_manual_roll = false;
+        training_manual_pitch = false;
+        
+        // if the roll is past the set roll limit, then
+        // we set target roll to the limit
+        if (ahrs.roll_sensor >= roll_limit_cd) {
+            nav_roll_cd = roll_limit_cd;
+        } else if (ahrs.roll_sensor <= -roll_limit_cd) {
+            nav_roll_cd = -roll_limit_cd;                
+        } else {
+            training_manual_roll = true;
+            nav_roll_cd = 0;
+        }
+        
+        // if the pitch is past the set pitch limits, then
+        // we set target pitch to the limit
+        if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
+            nav_pitch_cd = aparm.pitch_limit_max_cd;
+        } else if (ahrs.pitch_sensor <= pitch_limit_min_cd) {
+            nav_pitch_cd = pitch_limit_min_cd;
+        } else {
+            training_manual_pitch = true;
+            nav_pitch_cd = 0;
+        }
+        if (fly_inverted()) {
+            nav_pitch_cd = -nav_pitch_cd;
+        }
+        break;
+    }
+
+    case ACRO: {
+        // handle locked/unlocked control
+        if (acro_state.locked_roll) {
+            nav_roll_cd = acro_state.locked_roll_err;
+        } else {
+            nav_roll_cd = ahrs.roll_sensor;
+        }
+        if (acro_state.locked_pitch) {
+            nav_pitch_cd = acro_state.locked_pitch_cd;
+        } else {
+            nav_pitch_cd = ahrs.pitch_sensor;
+        }
+        break;
+    }
+
+    case AUTOTUNE:
+    case FLY_BY_WIRE_A: {
+        // set nav_roll and nav_pitch using sticks
+        nav_roll_cd  = channel_roll->norm_input() * roll_limit_cd;
+        nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
+        update_load_factor();
+        float pitch_input = channel_pitch->norm_input();
+        if (pitch_input > 0) {
+            nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
+        } else {
+            nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
+        }
+        adjust_nav_pitch_throttle();
+        nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
+        if (fly_inverted()) {
+            nav_pitch_cd = -nav_pitch_cd;
+        }
+        if (failsafe.ch3_failsafe && g.short_fs_action == 2) {
+            // FBWA failsafe glide
+            nav_roll_cd = 0;
+            nav_pitch_cd = 0;
+            channel_throttle->servo_out = 0;
+        }
+        if (g.fbwa_tdrag_chan > 0) {
+            // check for the user enabling FBWA taildrag takeoff mode
+            bool tdrag_mode = (hal.rcin->read(g.fbwa_tdrag_chan-1) > 1700);
+            if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) {
+                if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) {
+                    auto_state.fbwa_tdrag_takeoff_mode = true;
+                    gcs_send_text_P(SEVERITY_LOW, PSTR("FBWA tdrag mode\n"));
+                }
+            }
+        }
+        break;
+    }
+
+    case FLY_BY_WIRE_B:
+        // Thanks to Yury MonZon for the altitude limit code!
+        nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
+        nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
+        update_load_factor();
+        update_fbwb_speed_height();
+        break;
+        
+    case CRUISE:
+        /*
+          in CRUISE mode we use the navigation code to control
+          roll when heading is locked. Heading becomes unlocked on
+          any aileron or rudder input
+        */
+        if ((channel_roll->control_in != 0 ||
+             rudder_input != 0)) {                
+            cruise_state.locked_heading = false;
+            cruise_state.lock_timer_ms = 0;
+        }                 
+        
+        if (!cruise_state.locked_heading) {
+            nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
+            nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
+            update_load_factor();
+        } else {
+            calc_nav_roll();
+        }
+        update_fbwb_speed_height();
+        break;
+        
+    case STABILIZE:
+        nav_roll_cd        = 0;
+        nav_pitch_cd       = 0;
+        // throttle is passthrough
+        break;
+        
+    case CIRCLE:
+        // we have no GPS installed and have lost radio contact
+        // or we just want to fly around in a gentle circle w/o GPS,
+        // holding altitude at the altitude we set when we
+        // switched into the mode
+        nav_roll_cd  = roll_limit_cd / 3;
+        update_load_factor();
+        calc_nav_pitch();
+        calc_throttle();
+        break;
+
+    case MANUAL:
+        // servo_out is for Sim control only
+        // ---------------------------------
+        channel_roll->servo_out = channel_roll->pwm_to_angle();
+        channel_pitch->servo_out = channel_pitch->pwm_to_angle();
+        steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle();
+        break;
+        //roll: -13788.000,  pitch: -13698.000,   thr: 0.000, rud: -13742.000
+        
+    case INITIALISING:
+        // handled elsewhere
+        break;
+    }
+}
+
+void Plane::update_navigation()
+{
+    // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
+    // ------------------------------------------------------------------------
+
+    // distance and bearing calcs only
+    switch(control_mode) {
+    case AUTO:
+        update_commands();
+        break;
+            
+    case RTL:
+        if (g.rtl_autoland == 1 &&
+            !auto_state.checked_for_autoland &&
+            nav_controller->reached_loiter_target() && 
+            labs(altitude_error_cm) < 1000) {
+            // we've reached the RTL point, see if we have a landing sequence
+            jump_to_landing_sequence();
+
+            // prevent running the expensive jump_to_landing_sequence
+            // on every loop
+            auto_state.checked_for_autoland = true;
+        }
+        else if (g.rtl_autoland == 2 &&
+            !auto_state.checked_for_autoland) {
+            // Go directly to the landing sequence
+            jump_to_landing_sequence();
+
+            // prevent running the expensive jump_to_landing_sequence
+            // on every loop
+            auto_state.checked_for_autoland = true;
+        }
+        // fall through to LOITER
+
+    case LOITER:
+    case GUIDED:
+        // allow loiter direction to be changed in flight
+        if (g.loiter_radius < 0) {
+            loiter.direction = -1;
+        } else {
+            loiter.direction = 1;
+        }
+        update_loiter();
+        break;
+
+    case CRUISE:
+        update_cruise();
+        break;
+
+    case MANUAL:
+    case STABILIZE:
+    case TRAINING:
+    case INITIALISING:
+    case ACRO:
+    case FLY_BY_WIRE_A:
+    case AUTOTUNE:
+    case FLY_BY_WIRE_B:
+    case CIRCLE:
+        // nothing to do
+        break;
+    }
+}
+
+/*
+  set the flight stage
+ */
+void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs) 
+{
+    //if just now entering land flight stage
+    if (fs == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
+        flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
+
+#if GEOFENCE_ENABLED == ENABLED 
+        if (g.fence_autoenable == 1) {
+            if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
+                gcs_send_text_P(SEVERITY_HIGH, PSTR("Disable fence failed (autodisable)"));
+            } else {
+                gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence disabled (autodisable)"));
+            }
+        } else if (g.fence_autoenable == 2) {
+            if (! geofence_set_floor_enabled(false)) {
+                gcs_send_text_P(SEVERITY_HIGH, PSTR("Disable fence floor failed (autodisable)"));
+            } else {
+                gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence floor disabled (auto disable)"));
+            }
+        }
+#endif
+    }
+    
+    flight_stage = fs;
+}
+
+void Plane::update_alt()
+{
+    barometer.update();
+    if (should_log(MASK_LOG_IMU)) {
+        Log_Write_Baro();
+    }
+
+    geofence_check(true);
+
+    update_flight_stage();
+}
+
+/*
+  recalculate the flight_stage
+ */
+void Plane::update_flight_stage(void)
+{
+    // Update the speed & height controller states
+    if (auto_throttle_mode && !throttle_suppressed) {        
+        if (control_mode==AUTO) {
+            if (auto_state.takeoff_complete == false) {
+                set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
+            } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND && 
+                       auto_state.land_complete == true) {
+                set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
+            } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
+                set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH); 
+            } else {
+                set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
+            }
+        } else {
+            set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
+        }
+
+        SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
+                                                 target_airspeed_cm,
+                                                 flight_stage,
+                                                 auto_state.takeoff_pitch_cd,
+                                                 throttle_nudge,
+                                                 tecs_hgt_afe(),
+                                                 aerodynamic_load_factor);
+        if (should_log(MASK_LOG_TECS)) {
+            Log_Write_TECS_Tuning();
+        }
+    }
+
+    // tell AHRS the airspeed to true airspeed ratio
+    airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
+}
+
+
+
+/*
+  Do we think we are flying?
+  Probabilistic method where a bool is low-passed and considered a probability.
+*/
+void Plane::determine_is_flying(void)
+{
+    float aspeed;
+    bool isFlyingBool;
+
+    bool airspeedMovement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= 5);
+
+    // If we don't have a GPS lock then don't use GPS for this test
+    bool gpsMovement = (gps.status() < AP_GPS::GPS_OK_FIX_2D ||
+                        gps.ground_speed() >= 5);
+
+
+    if (hal.util->get_soft_armed()) {
+        // when armed, we need overwhelming evidence that we ARE NOT flying
+        isFlyingBool = airspeedMovement || gpsMovement;
+
+        /*
+          make is_flying() more accurate for landing approach
+         */
+        if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
+            fabsf(auto_state.land_sink_rate) > 0.2f) {
+            isFlyingBool = true;
+        }
+    } else {
+        // when disarmed, we need overwhelming evidence that we ARE flying
+        isFlyingBool = airspeedMovement && gpsMovement;
+    }
+
+    // low-pass the result.
+    isFlyingProbability = (0.6f * isFlyingProbability) + (0.4f * (float)isFlyingBool);
+
+    /*
+      update last_flying_ms so we always know how long we have not
+      been flying for. This helps for crash detection and auto-disarm
+     */
+    if (is_flying()) {
+        auto_state.last_flying_ms = hal.scheduler->millis();
+    }
+}
+
+/*
+  return true if we think we are flying. This is a probabilistic
+  estimate, and needs to be used very carefully. Each use case needs
+  to be thought about individually.
+ */
+bool Plane::is_flying(void)
+{
+    if (hal.util->get_soft_armed()) {
+        // when armed, assume we're flying unless we probably aren't
+        return (isFlyingProbability >= 0.1f);
+    }
+
+    // when disarmed, assume we're not flying unless we probably are
+    return (isFlyingProbability >= 0.9f);
+}
+
+
+#if OPTFLOW == ENABLED
+// called at 50hz
+void Plane::update_optical_flow(void)
+{
+    static uint32_t last_of_update = 0;
+
+    // exit immediately if not enabled
+    if (!optflow.enabled()) {
+        return;
+    }
+
+    // read from sensor
+    optflow.update();
+
+    // write to log and send to EKF if new data has arrived
+    if (optflow.last_update() != last_of_update) {
+        last_of_update = optflow.last_update();
+        uint8_t flowQuality = optflow.quality();
+        Vector2f flowRate = optflow.flowRate();
+        Vector2f bodyRate = optflow.bodyRate();
+        ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update);
+        Log_Write_Optflow();
+    }
+}
+#endif
+
+/*
+  compatibility with old pde style build
+ */
+void setup(void);
+void loop(void);
+
+void setup(void)
+{
+    plane.setup();
+}
+void loop(void)
+{
+    plane.loop();
+}
+
+AP_HAL_MAIN();

ArduPlane/Attitude.cpp

@@ -1,16 +1,13 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-//****************************************************************
+#include "Plane.h"
-// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
-//****************************************************************
-
 
 /*
   get a speed scaling number for control surfaces. This is applied to
   PIDs to change the scaling of the PID with speed. At high speed we
   move the surfaces less, and at low speeds we move them more.
  */
-static float get_speed_scaler(void)
+float Plane::get_speed_scaler(void)
 {
     float aspeed, speed_scaler;
     if (ahrs.airspeed_estimate(&aspeed)) {
@@ -39,7 +36,7 @@ static float get_speed_scaler(void)
 /*
   return true if the current settings and mode should allow for stick mixing
  */
-static bool stick_mixing_enabled(void)
+bool Plane::stick_mixing_enabled(void)
 {
     if (auto_throttle_mode) {
         // we're in an auto mode. Check the stick mixing flag
@@ -69,7 +66,7 @@ static bool stick_mixing_enabled(void)
   previously set nav_roll calculates roll servo_out to try to
   stabilize the plane at the given roll
  */
-static void stabilize_roll(float speed_scaler)
+void Plane::stabilize_roll(float speed_scaler)
 {
     if (fly_inverted()) {
         // we want to fly upside down. We need to cope with wrap of
@@ -95,7 +92,7 @@ static void stabilize_roll(float speed_scaler)
   previously set nav_pitch and calculates servo_out values to try to
   stabilize the plane at the given attitude.
  */
-static void stabilize_pitch(float speed_scaler)
+void Plane::stabilize_pitch(float speed_scaler)
 {
     int8_t force_elevator = takeoff_tail_hold();
     if (force_elevator != 0) {
@@ -120,7 +117,7 @@ static void stabilize_pitch(float speed_scaler)
   controller as it increases the influence of the users stick input,
   allowing the user full deflection if needed
  */
-static void stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
+void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
 {
     float ch_inf;
         
@@ -135,7 +132,7 @@ static void stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
 /*
   this gives the user control of the aircraft in stabilization modes
  */
-static void stabilize_stick_mixing_direct()
+void Plane::stabilize_stick_mixing_direct()
 {
     if (!stick_mixing_enabled() ||
         control_mode == ACRO ||
@@ -154,7 +151,7 @@ static void stabilize_stick_mixing_direct()
   this gives the user control of the aircraft in stabilization modes
   using FBW style controls
  */
-static void stabilize_stick_mixing_fbw()
+void Plane::stabilize_stick_mixing_fbw()
 {
     if (!stick_mixing_enabled() ||
         control_mode == ACRO ||
@@ -204,7 +201,7 @@ static void stabilize_stick_mixing_fbw()
     - rate controlled with ground steering
     - yaw control for coordinated flight    
  */
-static void stabilize_yaw(float speed_scaler)
+void Plane::stabilize_yaw(float speed_scaler)
 {
     if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
         // in land final setup for ground steering
@@ -245,7 +242,7 @@ static void stabilize_yaw(float speed_scaler)
 /*
   a special stabilization function for training mode
  */
-static void stabilize_training(float speed_scaler)
+void Plane::stabilize_training(float speed_scaler)
 {
     if (training_manual_roll) {
         channel_roll->servo_out = channel_roll->control_in;
@@ -278,7 +275,7 @@ static void stabilize_training(float speed_scaler)
   this is the ACRO mode stabilization function. It does rate
   stabilization on roll and pitch axes
  */
-static void stabilize_acro(float speed_scaler)
+void Plane::stabilize_acro(float speed_scaler)
 {
     float roll_rate = (channel_roll->control_in/4500.0f) * g.acro_roll_rate;
     float pitch_rate = (channel_pitch->control_in/4500.0f) * g.acro_pitch_rate;
@@ -345,7 +342,7 @@ static void stabilize_acro(float speed_scaler)
 /*
   main stabilization function for all 3 axes
  */
-static void stabilize()
+void Plane::stabilize()
 {
     if (control_mode == MANUAL) {
         // nothing to do
@@ -391,7 +388,7 @@ static void stabilize()
 }
 
 
-static void calc_throttle()
+void Plane::calc_throttle()
 {
     if (aparm.throttle_cruise <= 1) {
         // user has asked for zero throttle - this may be done by a
@@ -411,7 +408,7 @@ static void calc_throttle()
 /*
   calculate yaw control for coordinated flight
  */
-static void calc_nav_yaw_coordinated(float speed_scaler)
+void Plane::calc_nav_yaw_coordinated(float speed_scaler)
 {
     bool disable_integrator = false;
     if (control_mode == STABILIZE && rudder_input != 0) {
@@ -428,7 +425,7 @@ static void calc_nav_yaw_coordinated(float speed_scaler)
 /*
   calculate yaw control for ground steering with specific course
  */
-static void calc_nav_yaw_course(void)
+void Plane::calc_nav_yaw_course(void)
 {
     // holding a specific navigation course on the ground. Used in
     // auto-takeoff and landing
@@ -443,7 +440,7 @@ static void calc_nav_yaw_course(void)
 /*
   calculate yaw control for ground steering
  */
-static void calc_nav_yaw_ground(void)
+void Plane::calc_nav_yaw_ground(void)
 {
     if (gps.ground_speed() < 1 && 
         channel_throttle->control_in == 0 &&
@@ -484,7 +481,7 @@ static void calc_nav_yaw_ground(void)
 /*
   calculate a new nav_pitch_cd from the speed height controller
  */
-static void calc_nav_pitch()
+void Plane::calc_nav_pitch()
 {
     // Calculate the Pitch of the plane
     // --------------------------------
@@ -496,7 +493,7 @@ static void calc_nav_pitch()
 /*
   calculate a new nav_roll_cd from the navigation controller
  */
-static void calc_nav_roll()
+void Plane::calc_nav_roll()
 {
     nav_roll_cd = nav_controller->nav_roll_cd();
     update_load_factor();
@@ -507,7 +504,7 @@ static void calc_nav_roll()
 /*****************************************
 * Throttle slew limit
 *****************************************/
-static void throttle_slew_limit(int16_t last_throttle)
+void Plane::throttle_slew_limit(int16_t last_throttle)
 {
     uint8_t slewrate = aparm.throttle_slewrate;
     if (control_mode==AUTO && auto_state.takeoff_complete == false && g.takeoff_throttle_slewrate != 0) {
@@ -528,7 +525,7 @@ static void throttle_slew_limit(int16_t last_throttle)
 /*****************************************
 Flap slew limit
 *****************************************/
-static void flap_slew_limit(int8_t &last_value, int8_t &new_value)
+void Plane::flap_slew_limit(int8_t &last_value, int8_t &new_value)
 {
     uint8_t slewrate = g.flap_slewrate;
     // if slew limit rate is set to zero then do not slew limit
@@ -556,7 +553,7 @@ static void flap_slew_limit(int8_t &last_value, int8_t &new_value)
    *       OR
    *       5 - Home location is not set
 */
-static bool suppress_throttle(void)
+bool Plane::suppress_throttle(void)
 {
     if (!throttle_suppressed) {
         // we've previously met a condition for unsupressing the throttle
@@ -614,7 +611,7 @@ static bool suppress_throttle(void)
 /*
   implement a software VTail or elevon mixer. There are 4 different mixing modes
  */
-static void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out)
+void Plane::channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out)
 {
     int16_t c1, c2;
     int16_t v1, v2;
@@ -659,7 +656,7 @@ static void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_
 /*
   setup flaperon output channels
  */
-static void flaperon_update(int8_t flap_percent)
+void Plane::flaperon_update(int8_t flap_percent)
 {
     if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon1) ||
         !RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon2)) {
@@ -688,7 +685,7 @@ static void flaperon_update(int8_t flap_percent)
 /*****************************************
 * Set the flight control servos based on the current calculated values
 *****************************************/
-static void set_servos(void)
+void Plane::set_servos(void)
 {
     int16_t last_throttle = channel_throttle->radio_out;
 
@@ -980,20 +977,18 @@ static void set_servos(void)
     RC_Channel_aux::output_ch_all();
 }
 
-static bool demoing_servos;
+void Plane::demo_servos(uint8_t i) 
-
-static void demo_servos(uint8_t i) 
 {
     while(i > 0) {
         gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
         demoing_servos = true;
         servo_write(1, 1400);
-        mavlink_delay(400);
+        hal.scheduler->delay(400);
         servo_write(1, 1600);
-        mavlink_delay(200);
+        hal.scheduler->delay(200);
         servo_write(1, 1500);
         demoing_servos = false;
-        mavlink_delay(400);
+        hal.scheduler->delay(400);
         i--;
     }
 }
@@ -1004,7 +999,7 @@ static void demo_servos(uint8_t i)
   automatically pitch down a little when at low throttle. It makes
   FBWA landings without stalling much easier.
  */
-static void adjust_nav_pitch_throttle(void)
+void Plane::adjust_nav_pitch_throttle(void)
 {
     uint8_t throttle = throttle_percentage();
     if (throttle < aparm.throttle_cruise) {
@@ -1019,7 +1014,7 @@ static void adjust_nav_pitch_throttle(void)
   ensure that the load factor does not take us below the sustainable
   airspeed
  */
-static void update_load_factor(void)
+void Plane::update_load_factor(void)
 {
     float demanded_roll = fabsf(nav_roll_cd*0.01f);
     if (demanded_roll > 85) {

ArduPlane/Log.cpp

@@ -1,33 +1,28 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Plane.h"
+
 #if LOGGING_ENABLED == ENABLED
 
 #if CLI_ENABLED == ENABLED
 // Code to Write and Read packets from DataFlash.log memory
 // Code to interact with the user to dump or erase logs
 
-// These are function definitions so the Menu can be constructed before the functions
-// are defined below. Order matters to the compiler.
-static int8_t   dump_log(uint8_t argc,                  const Menu::arg *argv);
-static int8_t   erase_logs(uint8_t argc,                const Menu::arg *argv);
-static int8_t   select_logs(uint8_t argc,               const Menu::arg *argv);
-
 // Creates a constant array of structs representing menu options
 // and stores them in Flash memory, not RAM.
 // User enters the string in the console to call the functions on the right.
 // See class Menu in AP_Coommon for implementation details
 static const struct Menu::command log_menu_commands[] PROGMEM = {
-    {"dump",        dump_log},
+    {"dump",        MENU_FUNC(dump_log)},
-    {"erase",       erase_logs},
+    {"erase",       MENU_FUNC(erase_logs)},
-    {"enable",      select_logs},
+    {"enable",      MENU_FUNC(select_logs)},
-    {"disable",     select_logs}
+    {"disable",     MENU_FUNC(select_logs)}
 };
 
 // A Macro to create the Menu
-MENU2(log_menu, "Log", log_menu_commands, print_log_menu);
+MENU2(log_menu, "Log", log_menu_commands, MENU_FUNC(print_log_menu));
 
-static bool
+bool Plane::print_log_menu(void)
-print_log_menu(void)
 {
     cliSerial->println_P(PSTR("logs enabled: "));
 
@@ -63,39 +58,37 @@ print_log_menu(void)
     return(true);
 }
 
-static int8_t
+int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv)
-dump_log(uint8_t argc, const Menu::arg *argv)
 {
-    int16_t dump_log;
+    int16_t dump_log_num;
     uint16_t dump_log_start;
     uint16_t dump_log_end;
     uint16_t last_log_num;
 
     // check that the requested log number can be read
-    dump_log = argv[1].i;
+    dump_log_num = argv[1].i;
     last_log_num = DataFlash.find_last_log();
 
-    if (dump_log == -2) {
+    if (dump_log_num == -2) {
         DataFlash.DumpPageInfo(cliSerial);
         return(-1);
-    } else if (dump_log <= 0) {
+    } else if (dump_log_num <= 0) {
         cliSerial->printf_P(PSTR("dumping all\n"));
         Log_Read(0, 1, 0);
         return(-1);
     } else if ((argc != 2)
-               || ((uint16_t)dump_log > last_log_num))
+               || ((uint16_t)dump_log_num > last_log_num))
     {
         cliSerial->printf_P(PSTR("bad log number\n"));
         return(-1);
     }
 
-    DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
+    DataFlash.get_log_boundaries(dump_log_num, dump_log_start, dump_log_end);
-    Log_Read((uint16_t)dump_log, dump_log_start, dump_log_end);
+    Log_Read((uint16_t)dump_log_num, dump_log_start, dump_log_end);
     return 0;
 }
 
-static int8_t
+int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv)
-erase_logs(uint8_t argc, const Menu::arg *argv)
 {
     in_mavlink_delay = true;
     do_erase_logs();
@@ -103,8 +96,7 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
     return 0;
 }
 
-static int8_t
+int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv)
-select_logs(uint8_t argc, const Menu::arg *argv)
 {
     uint32_t bits;
 
@@ -151,8 +143,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
     return(0);
 }
 
-static int8_t
+int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv)
-process_logs(uint8_t argc, const Menu::arg *argv)
 {
     log_menu.run();
     return 0;
@@ -160,7 +151,7 @@ process_logs(uint8_t argc, const Menu::arg *argv)
 
 #endif // CLI_ENABLED == ENABLED
 
-static void do_erase_logs(void)
+void Plane::do_erase_logs(void)
 {
     gcs_send_text_P(SEVERITY_LOW, PSTR("Erasing logs"));
     DataFlash.EraseAll();
@@ -169,7 +160,7 @@ static void do_erase_logs(void)
 
 
 // Write an attitude packet
-static void Log_Write_Attitude(void)
+void Plane::Log_Write_Attitude(void)
 {
     Vector3f targets;       // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude
     targets.x = nav_roll_cd;
@@ -206,11 +197,11 @@ struct PACKED log_Performance {
 };
 
 // Write a performance monitoring packet. Total length : 19 bytes
-static void Log_Write_Performance()
+void Plane::Log_Write_Performance()
 {
     struct log_Performance pkt = {
         LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
-        loop_time       : millis() - perf_mon_timer,
+        loop_time       : hal.scheduler->millis() - perf_mon_timer,
         main_loop_count : mainLoop_count,
         g_dt_max        : G_Dt_max,
         gyro_drift_x    : (int16_t)(ahrs.get_gyro_drift().x * 1000),
@@ -223,7 +214,7 @@ static void Log_Write_Performance()
 }
 
 // Write a mission command. Total length : 36 bytes
-static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd)
+void Plane::Log_Write_Cmd(const AP_Mission::Mission_Command &cmd)
 {
     mavlink_mission_item_t mav_cmd = {};
     AP_Mission::mission_cmd_to_mavlink(cmd,mav_cmd);
@@ -236,7 +227,7 @@ struct PACKED log_Startup {
     uint16_t command_total;
 };
 
-static void Log_Write_Startup(uint8_t type)
+void Plane::Log_Write_Startup(uint8_t type)
 {
     struct log_Startup pkt = {
         LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
@@ -249,7 +240,7 @@ static void Log_Write_Startup(uint8_t type)
     Log_Write_EntireMission();
 }
 
-static void Log_Write_EntireMission()
+void Plane::Log_Write_EntireMission()
 {
     DataFlash.Log_Write_Message_P(PSTR("New mission"));
 
@@ -274,7 +265,7 @@ struct PACKED log_Control_Tuning {
 };
 
 // Write a control tuning packet. Total length : 22 bytes
-static void Log_Write_Control_Tuning()
+void Plane::Log_Write_Control_Tuning()
 {
     Vector3f accel = ins.get_accel();
     struct log_Control_Tuning pkt = {
@@ -292,7 +283,7 @@ static void Log_Write_Control_Tuning()
 }
 
 // Write a TECS tuning packet
-static void Log_Write_TECS_Tuning(void)
+void Plane::Log_Write_TECS_Tuning(void)
 {
     SpdHgt_Controller->log_data(DataFlash, LOG_TECS_MSG);
 }
@@ -311,7 +302,7 @@ struct PACKED log_Nav_Tuning {
 };
 
 // Write a navigation tuning packe
-static void Log_Write_Nav_Tuning()
+void Plane::Log_Write_Nav_Tuning()
 {
     struct log_Nav_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
@@ -337,7 +328,7 @@ struct PACKED log_Status {
     uint8_t safety;
 };
 
-static void Log_Write_Status()
+void Plane::Log_Write_Status()
 {
     struct log_Status pkt = {
         LOG_PACKET_HEADER_INIT(LOG_STATUS_MSG)
@@ -364,7 +355,7 @@ struct PACKED log_Sonar {
 };
 
 // Write a sonar packet
-static void Log_Write_Sonar()
+void Plane::Log_Write_Sonar()
 {
     struct log_Sonar pkt = {
         LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
@@ -392,7 +383,7 @@ struct PACKED log_Optflow {
 
 #if OPTFLOW == ENABLED
 // Write an optical flow packet
-static void Log_Write_Optflow()
+void Plane::Log_Write_Optflow()
 {
     // exit immediately if not enabled
     if (!optflow.enabled()) {
@@ -420,7 +411,7 @@ struct PACKED log_Arm_Disarm {
     uint16_t arm_checks;
 };
 
-static void Log_Write_Current()
+void Plane::Log_Write_Current()
 {
     DataFlash.Log_Write_Current(battery, channel_throttle->control_in);
 
@@ -428,7 +419,7 @@ static void Log_Write_Current()
     DataFlash.Log_Write_Power();
 }
 
-static void Log_Arm_Disarm() {
+void Plane::Log_Arm_Disarm() {
     struct log_Arm_Disarm pkt = {
         LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG),
         time_ms                 : hal.scheduler->millis(),
@@ -438,29 +429,29 @@ static void Log_Arm_Disarm() {
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
-static void Log_Write_GPS(uint8_t instance)
+void Plane::Log_Write_GPS(uint8_t instance)
 {
     DataFlash.Log_Write_GPS(gps, instance, current_loc.alt - ahrs.get_home().alt);
 }
 
-static void Log_Write_IMU() 
+void Plane::Log_Write_IMU() 
 {
     DataFlash.Log_Write_IMU(ins);
 }
 
-static void Log_Write_RC(void)
+void Plane::Log_Write_RC(void)
 {
     DataFlash.Log_Write_RCIN();
     DataFlash.Log_Write_RCOUT();
 }
 
-static void Log_Write_Baro(void)
+void Plane::Log_Write_Baro(void)
 {
     DataFlash.Log_Write_Baro(barometer);
 }
 
 // Write a AIRSPEED packet
-static void Log_Write_Airspeed(void)
+void Plane::Log_Write_Airspeed(void)
 {
     DataFlash.Log_Write_Airspeed(airspeed);
 }
@@ -492,7 +483,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
 
 #if CLI_ENABLED == ENABLED
 // Read the DataFlash.log memory : Packet Parser
-static void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
+void Plane::Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
 {
     cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING
                              "\nFree RAM: %u\n"),
@@ -501,13 +492,13 @@ static void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
     cliSerial->println_P(PSTR(HAL_BOARD_NAME));
 
 	DataFlash.LogReadProcess(log_num, start_page, end_page, 
-                             print_flight_mode,
+                             AP_HAL_MEMBERPROC(&Plane::print_flight_mode),
                              cliSerial);
 }
 #endif // CLI_ENABLED
 
 // start a new log
-static void start_logging() 
+void Plane::start_logging() 
 {
     DataFlash.StartNewLog();
     DataFlash.Log_Write_Message_P(PSTR(FIRMWARE_STRING));
@@ -522,31 +513,29 @@ static void start_logging()
     }
 }
 
+/*
+  initialise logging subsystem
+ */
+void Plane::log_init(void)
+{
+    DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
+    if (!DataFlash.CardInserted()) {
+        gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash card inserted"));
+        g.log_bitmask.set(0);
+    } else if (DataFlash.NeedErase()) {
+        gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
+        do_erase_logs();
+        for (uint8_t i=0; i<num_gcs; i++) {
+            gcs[i].reset_cli_timeout();
+        }
+    }
+    arming.set_logging_available(DataFlash.CardInserted());
+}
+
 #else // LOGGING_ENABLED
 
-// dummy functions
+int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv) 
-static void Log_Write_Startup(uint8_t type) {}
+{
-static void Log_Write_EntireMission() {}
-static void Log_Write_Current() {}
-static void Log_Write_Nav_Tuning() {}
-static void Log_Write_TECS_Tuning() {}
-static void Log_Write_Performance() {}
-static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd) {}
-static void Log_Write_Attitude() {}
-static void Log_Write_Control_Tuning() {}
-static void Log_Write_GPS(uint8_t instance) {}
-static void Log_Write_IMU() {}
-static void Log_Write_RC() {}
-static void Log_Write_Airspeed(void) {}
-static void Log_Write_Baro(void) {}
-static void Log_Write_Status() {}
-static void Log_Write_Sonar() {}
-#if OPTFLOW == ENABLED
-static void Log_Write_Optflow() {}
-#endif
-static void Log_Arm_Disarm() {}
-
-static int8_t process_logs(uint8_t argc, const Menu::arg *argv) {
     return 0;
 }
 

ArduPlane/Parameters.cpp

@@ -1,17 +1,19 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Plane.h"
+
 /*
  *  ArduPlane parameter definitions
  *
  */
 
-#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value : def} }
+#define GSCALAR(v, name, def) { plane.g.v.vtype, name, Parameters::k_param_ ## v, &plane.g.v, {def_value : def} }
-#define ASCALAR(v, name, def) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.v, {def_value : def} }
+#define ASCALAR(v, name, def) { plane.aparm.v.vtype, name, Parameters::k_param_ ## v, &plane.aparm.v, {def_value : def} }
-#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info : class::var_info} }
+#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &plane.g.v, {group_info : class::var_info} }
-#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info : class::var_info} }
+#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &plane.v, {group_info : class::var_info} }
-#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} }
+#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &plane.v, {group_info : class::var_info} }
 
-const AP_Param::Info var_info[] PROGMEM = {
+const AP_Param::Info Plane::var_info[] PROGMEM = {
     // @Param: FORMAT_VERSION
     // @DisplayName: Eeprom format version number
     // @Description: This value is incremented when changes are made to the eeprom format
@@ -1224,7 +1226,7 @@ const AP_Param::ConversionInfo conversion_table[] PROGMEM = {
     { Parameters::k_param_serial2_baud,       0,      AP_PARAM_INT16, "SERIAL2_BAUD" },
 };
 
-static void load_parameters(void)
+void Plane::load_parameters(void)
 {
     if (!AP_Param::check_var_info()) {
         cliSerial->printf_P(PSTR("Bad parameter table\n"));        
@@ -1241,10 +1243,10 @@ static void load_parameters(void)
         g.format_version.set_and_save(Parameters::k_format_version);
         cliSerial->println_P(PSTR("done."));
     } else {
-        uint32_t before = micros();
+        uint32_t before = hal.scheduler->micros();
         // Load all auto-loaded EEPROM variables
         AP_Param::load_all();
         AP_Param::convert_old_parameters(&conversion_table[0], sizeof(conversion_table)/sizeof(conversion_table[0]));
-        cliSerial->printf_P(PSTR("load_all took %luus\n"), micros() - before);
+        cliSerial->printf_P(PSTR("load_all took %luus\n"), hal.scheduler->micros() - before);
     }
 }

ArduPlane/Plane.cpp

@@ -0,0 +1,93 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Plane.h"
+
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  constructor for main Plane class
+ */
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+Plane::Plane(void) :
+    ins_sample_rate(AP_InertialSensor::RATE_50HZ),
+#if defined(HAL_BOARD_LOG_DIRECTORY)
+    DataFlash(HAL_BOARD_LOG_DIRECTORY),
+#endif
+    flight_modes(&g.flight_mode1),
+#if AP_AHRS_NAVEKF_AVAILABLE
+    ahrs(ins, barometer, gps, rng),
+#else
+    ahrs(ins, barometer, gps),
+#endif
+    L1_controller(ahrs),
+    TECS_controller(ahrs, aparm),
+    rollController(ahrs, aparm, DataFlash),
+    pitchController(ahrs, aparm, DataFlash),
+    yawController(ahrs, aparm),
+    steerController(ahrs),
+    num_gcs(MAVLINK_COMM_NUM_BUFFERS),
+    nav_controller(&L1_controller),
+    SpdHgt_Controller(&TECS_controller),
+    ServoRelayEvents(relay),
+#if CAMERA == ENABLED
+    camera(&relay),
+#endif
+    rally(ahrs),
+    control_mode(INITIALISING),
+    previous_mode(INITIALISING),
+    oldSwitchPosition(254),
+    ground_start_count(5),
+#if FRSKY_TELEM_ENABLED == ENABLED
+    frsky_telemetry(ahrs, battery),
+#endif
+    airspeed(aparm),
+    flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL),
+    aerodynamic_load_factor(1.0f),
+    mission(ahrs, 
+            AP_HAL_MEMBERPROC(&Plane::start_command_callback), 
+            AP_HAL_MEMBERPROC(&Plane::verify_command_callback), 
+            AP_HAL_MEMBERPROC(&Plane::exit_mission_callback)),
+#if AP_TERRAIN_AVAILABLE
+    terrain(ahrs, mission, rally),
+#endif
+#if OBC_FAILSAFE == ENABLED
+    obc(mission, barometer, gps, rcmap),
+#endif
+    home(ahrs.get_home()),
+    G_Dt(0.02f),
+#if MOUNT == ENABLED
+    camera_mount(ahrs, current_loc),
+#endif
+    arming(ahrs, barometer, compass, home_is_set, AP_HAL_MEMBERPROC(&Plane::gcs_send_text_P)),
+    param_loader(var_info)
+{
+    elevon.trim1 = 1500;
+    elevon.trim2 = 1500;
+    elevon.ch1_temp = 1500;
+    elevon.ch2_temp = 1500;
+
+    steer_state.hold_course_cd = -1;
+    steer_state.locked_course = false;
+    steer_state.locked_course_err = 0;
+
+    auto_state.takeoff_complete = true;
+    auto_state.next_wp_no_crosstrack = true;
+    auto_state.no_crosstrack = true;
+    auto_state.next_turn_angle = 90.0f;
+}
+
+Plane plane;

ArduPlane/Plane.h

@@ -0,0 +1,927 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#define THISFIRMWARE "ArduPlane V3.3.0beta2"
+/*
+   Lead developer: Andrew Tridgell
+ 
+   Authors:    Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher, Paul Riseborough, Brandon Jones, Jon Challinger
+   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, Yury MonZon
+
+   Please contribute your ideas! See http://dev.ardupilot.com for details
+
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+// Header includes
+////////////////////////////////////////////////////////////////////////////////
+
+#include <math.h>
+#include <stdarg.h>
+#include <stdio.h>
+
+#include <AP_HAL.h>
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_Menu.h>
+#include <AP_Param.h>
+#include <StorageManager.h>
+#include <AP_GPS.h>         // ArduPilot GPS library
+#include <AP_Baro.h>        // ArduPilot barometer library
+#include <AP_Compass.h>     // ArduPilot Mega Magnetometer Library
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <AP_ADC.h>         // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_ADC_AnalogSource.h>
+#include <AP_InertialSensor.h> // Inertial Sensor Library
+#include <AP_AHRS.h>         // ArduPilot Mega DCM Library
+#include <RC_Channel.h>     // RC Channel Library
+#include <AP_RangeFinder.h>     // Range finder library
+#include <Filter.h>                     // Filter library
+#include <AP_Buffer.h>      // APM FIFO Buffer
+#include <AP_Relay.h>       // APM relay
+#include <AP_Camera.h>          // Photo or video camera
+#include <AP_Airspeed.h>
+#include <AP_Terrain.h>
+
+#include <APM_OBC.h>
+#include <APM_Control.h>
+#include <AP_AutoTune.h>
+#include <GCS.h>
+#include <GCS_MAVLink.h>    // MAVLink GCS definitions
+#include <AP_SerialManager.h>   // Serial manager library
+#include <AP_Mount.h>           // Camera/Antenna mount
+#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
+#include <DataFlash.h>
+#include <SITL.h>
+#include <AP_Scheduler.h>       // main loop scheduler
+
+#include <AP_Navigation.h>
+#include <AP_L1_Control.h>
+#include <AP_RCMapper.h>        // RC input mapping library
+
+#include <AP_Vehicle.h>
+#include <AP_SpdHgtControl.h>
+#include <AP_TECS.h>
+#include <AP_NavEKF.h>
+#include <AP_Mission.h>     // Mission command library
+
+#include <AP_Notify.h>      // Notify library
+#include <AP_BattMonitor.h> // Battery monitor library
+
+#include <AP_Arming.h>
+#include <AP_BoardConfig.h>
+#include <AP_Frsky_Telem.h>
+#include <AP_ServoRelayEvents.h>
+
+#include <AP_Rally.h>
+
+#include <AP_OpticalFlow.h>     // Optical Flow library
+
+// Configuration
+#include "config.h"
+
+// Local modules
+#include "defines.h"
+
+#include "Parameters.h"
+
+#include <AP_HAL_AVR.h>
+#include <AP_HAL_SITL.h>
+#include <AP_HAL_PX4.h>
+#include <AP_HAL_FLYMAPLE.h>
+#include <AP_HAL_Linux.h>
+#include <AP_HAL_Empty.h>
+#include <AP_HAL_VRBRAIN.h>
+
+class Plane {
+public:
+    friend class GCS_MAVLINK;
+    friend class Parameters;
+
+    Plane(void);
+    void setup();
+    void loop();
+
+private:
+    // key aircraft parameters passed to multiple libraries
+    AP_Vehicle::FixedWing aparm;
+    AP_HAL::BetterStream* cliSerial;
+
+    // the rate we run the main loop 
+    const AP_InertialSensor::Sample_rate ins_sample_rate;
+
+    // Global parameters are all contained within the 'g' class.
+    Parameters g;
+
+    // main loop scheduler
+    AP_Scheduler scheduler;
+ 
+    // mapping between input channels
+    RCMapper rcmap;
+
+    // board specific config
+    AP_BoardConfig BoardConfig;
+
+    // primary control channels
+    RC_Channel *channel_roll;
+    RC_Channel *channel_pitch;
+    RC_Channel *channel_throttle;
+    RC_Channel *channel_rudder;
+
+    // notification object for LEDs, buzzers etc (parameter set to false disables external leds)
+    AP_Notify notify;
+
+    // DataFlash
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+    DataFlash_APM1 DataFlash;
+#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
+    DataFlash_APM2 DataFlash;
+#elif defined(HAL_BOARD_LOG_DIRECTORY)
+    DataFlash_File DataFlash;
+#else
+    // no dataflash driver
+    DataFlash_Empty DataFlash;
+#endif
+
+    // has a log download started?
+    bool in_log_download;
+
+    // scaled roll limit based on pitch
+    int32_t roll_limit_cd;
+    int32_t pitch_limit_min_cd;
+
+    // Sensors
+    AP_GPS gps;
+    RangeFinder rng;
+
+    // flight modes convenience array
+    AP_Int8 *flight_modes;
+
+    AP_Baro barometer;
+    Compass compass;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+    AP_ADC_ADS7844 apm1_adc;
+#endif
+
+    AP_InertialSensor ins;
+
+// Inertial Navigation EKF
+#if AP_AHRS_NAVEKF_AVAILABLE
+    AP_AHRS_NavEKF ahrs;
+#else
+    AP_AHRS_DCM ahrs;
+#endif
+
+    AP_L1_Control L1_controller;
+    AP_TECS TECS_controller;
+
+    // Attitude to servo controllers
+    AP_RollController  rollController;
+    AP_PitchController pitchController;
+    AP_YawController   yawController;
+    AP_SteerController steerController;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    SITL sitl;
+#endif
+
+    // Training mode
+    bool training_manual_roll;  // user has manual roll control
+    bool training_manual_pitch; // user has manual pitch control
+
+    /*
+      keep steering and rudder control separated until we update servos,
+      to allow for a separate wheel servo from rudder servo
+    */
+    struct {
+        bool ground_steering; // are we doing ground steering?
+        int16_t steering; // value for nose/tail wheel
+        int16_t rudder;   // value for rudder
+    } steering_control;
+
+    // should throttle be pass-thru in guided?
+    bool guided_throttle_passthru;
+
+    // are we doing calibration? This is used to allow heartbeat to
+    // external failsafe boards during baro and airspeed calibration
+    bool in_calibration;
+
+
+    // GCS selection
+    AP_SerialManager serial_manager;
+    const uint8_t num_gcs;
+    GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
+
+    // selected navigation controller
+    AP_Navigation *nav_controller;
+
+    // selected navigation controller
+    AP_SpdHgtControl *SpdHgt_Controller;
+
+    // Analog Inputs
+    // a pin for reading the receiver RSSI voltage. 
+    AP_HAL::AnalogSource *rssi_analog_source;
+
+    // rangefinder
+    RangeFinder rangefinder;
+
+    struct {
+        bool in_range;
+        float correction;
+        uint32_t last_correction_time_ms;
+        uint8_t in_range_count;
+    } rangefinder_state;
+
+    // Relay
+    AP_Relay relay;
+
+    // handle servo and relay events
+    AP_ServoRelayEvents ServoRelayEvents;
+
+    // Camera
+#if CAMERA == ENABLED
+    AP_Camera camera;
+#endif
+
+    // Optical flow sensor
+    OpticalFlow optflow;
+
+    // Rally Ponints
+    AP_Rally rally;
+
+    // remember if USB is connected, so we can adjust baud rate
+    bool usb_connected;
+
+    // This is the state of the flight control system
+    // There are multiple states defined such as MANUAL, FBW-A, AUTO
+    enum FlightMode control_mode;
+    enum FlightMode previous_mode;
+
+    // Used to maintain the state of the previous control switch position
+    // This is set to 254 when we need to re-read the switch
+    uint8_t oldSwitchPosition;
+
+    // This is used to enable the inverted flight feature
+    bool inverted_flight;
+
+    // This is used to enable the PX4IO override for testing
+    bool px4io_override_enabled;
+
+    struct {
+        // 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
+        uint16_t trim1;
+        uint16_t trim2;
+        // These are used in the calculation of elevon1_trim and elevon2_trim
+        uint16_t ch1_temp;
+        uint16_t ch2_temp;
+    } elevon;
+
+    // Failsafe
+    struct {
+        // 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
+        uint8_t ch3_failsafe:1;
+
+        // has the saved mode for failsafe been set?
+        uint8_t saved_mode_set:1;
+
+        // flag to hold whether battery low voltage threshold has been breached
+        uint8_t low_battery:1;
+
+        // saved flight mode
+        enum FlightMode saved_mode;
+
+        // A tracking variable for type of failsafe active
+        // Used for failsafe based on loss of RC signal or GCS signal
+        int16_t state;
+
+        // number of low ch3 values
+        uint8_t ch3_counter;
+
+        // the time when the last HEARTBEAT message arrived from a GCS
+        uint32_t last_heartbeat_ms;
+        
+        // A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
+        uint32_t ch3_timer_ms;
+        
+        uint32_t last_valid_rc_ms;
+    } failsafe;
+
+    // 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)
+    uint8_t ground_start_count;
+
+    // true if we have a position estimate from AHRS
+    bool have_position;
+
+    // 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.
+    int32_t target_airspeed_cm;
+
+    // The difference between current and desired airspeed.  Used in the pitch controller.  Centimeters per second.
+    float airspeed_error_cm;
+
+    // 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.
+    int16_t airspeed_nudge_cm;
+
+    // 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
+    int16_t throttle_nudge;
+
+    // receiver RSSI
+    uint8_t receiver_rssi;
+
+    // Ground speed
+    // The amount current ground speed is below min ground speed.  Centimeters per second
+    int32_t groundspeed_undershoot;
+
+    // Difference between current altitude and desired altitude.  Centimeters
+    int32_t altitude_error_cm;
+
+    // Battery Sensors
+    AP_BattMonitor battery;
+
+#if FRSKY_TELEM_ENABLED == ENABLED
+    // FrSky telemetry support
+    AP_Frsky_Telem frsky_telemetry;
+#endif
+
+    // Airspeed Sensors
+    AP_Airspeed airspeed;
+
+    // ACRO controller state
+    struct {
+        bool locked_roll;
+        bool locked_pitch;
+        float locked_roll_err;
+        int32_t locked_pitch_cd;
+    } acro_state;
+
+    // CRUISE controller state
+    struct {
+        bool locked_heading;
+        int32_t locked_heading_cd;
+        uint32_t lock_timer_ms;
+    } cruise_state;
+
+    // ground steering controller state
+    struct {
+        // Direction held during phases of takeoff and landing centidegrees
+        // A value of -1 indicates the course has not been set/is not in use
+        // this is a 0..36000 value, or -1 for disabled
+        int32_t hold_course_cd;
+
+        // locked_course and locked_course_cd are used in stabilize mode 
+        // when ground steering is active, and for steering in auto-takeoff
+        bool locked_course;
+        float locked_course_err;
+    } steer_state;
+
+    // flight mode specific
+    struct {
+        // Flag for using gps ground course instead of INS yaw.  Set false when takeoff command in process.
+        bool takeoff_complete:1;
+
+        // Flag to indicate if we have landed.
+        // Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
+        bool land_complete:1;
+
+        // should we fly inverted?
+        bool inverted_flight:1;
+
+        // should we disable cross-tracking for the next waypoint?
+        bool next_wp_no_crosstrack:1;
+
+        // should we use cross-tracking for this waypoint?
+        bool no_crosstrack:1;
+
+        // in FBWA taildragger takeoff mode
+        bool fbwa_tdrag_takeoff_mode:1;
+
+        // have we checked for an auto-land?
+        bool checked_for_autoland:1;
+
+        // denotes if a go-around has been commanded for landing
+        bool commanded_go_around:1;
+
+        // Altitude threshold to complete a takeoff command in autonomous
+        // modes.  Centimeters above home
+        int32_t takeoff_altitude_rel_cm;
+
+        // Minimum pitch to hold during takeoff command execution.  Hundredths of a degree
+        int16_t takeoff_pitch_cd;
+
+        // the highest airspeed we have reached since entering AUTO. Used
+        // to control ground takeoff
+        float highest_airspeed;
+        
+        // initial pitch. Used to detect if nose is rising in a tail dragger
+        int16_t initial_pitch_cd;
+        
+        // turn angle for next leg of mission
+        float next_turn_angle;
+
+        // filtered sink rate for landing
+        float land_sink_rate;
+
+        // time when we first pass min GPS speed on takeoff
+        uint32_t takeoff_speed_time_ms;
+        
+        // distance to next waypoint
+        float wp_distance;
+        
+        // proportion to next waypoint
+        float wp_proportion;
+        
+        // last time is_flying() returned true in milliseconds
+        uint32_t last_flying_ms;
+    } auto_state;
+
+    // true if we are in an auto-throttle mode, which means
+    // we need to run the speed/height controller
+    bool auto_throttle_mode;
+
+    // this controls throttle suppression in auto modes
+    bool throttle_suppressed;
+
+    AP_SpdHgtControl::FlightStage flight_stage;
+
+    // probability of aircraft is currently in flight. range from 0 to
+    // 1 where 1 is 100% sure we're in flight
+    float isFlyingProbability;
+
+    // Navigation control variables
+    // The instantaneous desired bank angle.  Hundredths of a degree
+    int32_t nav_roll_cd;
+
+    // The instantaneous desired pitch angle.  Hundredths of a degree
+    int32_t nav_pitch_cd;
+
+    // we separate out rudder input to allow for RUDDER_ONLY=1
+    int16_t rudder_input;
+
+    // the aerodymamic load factor. This is calculated from the demanded
+    // roll before the roll is clipped, using 1/sqrt(cos(nav_roll))
+    float aerodynamic_load_factor;
+
+    // a smoothed airspeed estimate, used for limiting roll angle
+    float smoothed_airspeed;
+
+    // Mission library
+    AP_Mission mission;
+
+    // terrain handling
+#if AP_TERRAIN_AVAILABLE
+    AP_Terrain terrain;
+#endif
+
+    // Outback Challenge Failsafe Support
+#if OBC_FAILSAFE == ENABLED
+    APM_OBC obc;
+#endif
+
+    /*
+      meta data to support counting the number of circles in a loiter
+    */
+    struct {
+        // previous target bearing, used to update sum_cd
+        int32_t old_target_bearing_cd;
+
+        // Total desired rotation in a loiter.  Used for Loiter Turns commands. 
+        int32_t total_cd;
+
+        // total angle completed in the loiter so far
+        int32_t sum_cd;
+
+        // Direction for loiter. 1 for clockwise, -1 for counter-clockwise
+        int8_t direction;
+
+        // start time of the loiter.  Milliseconds.
+        uint32_t start_time_ms;
+
+        // The amount of time we should stay in a loiter for the Loiter Time command.  Milliseconds.
+        uint32_t time_max_ms;
+    } loiter;
+
+
+    // Conditional command
+    // A value used in condition commands (eg delay, change alt, etc.)
+    // For example in a change altitude command, it is the altitude to change to.
+    int32_t condition_value;
+
+    // Sometimes there is a second condition required:
+    int32_t condition_value2;
+    // A starting value used to check the status of a conditional command.
+    // For example in a delay command the condition_start records that start time for the delay
+    uint32_t condition_start;
+    // A value used in condition commands.  For example the rate at which to change altitude.
+    int16_t condition_rate;
+
+    // 3D Location vectors
+    // Location structure defined in AP_Common
+    const struct Location &home;
+
+    // Flag for if we have g_gps lock and have set the home location in AHRS
+    enum HomeState home_is_set;
+
+    // The location of the previous waypoint.  Used for track following and altitude ramp calculations
+    Location prev_WP_loc;
+
+    // The plane's current location
+    struct Location current_loc;
+
+    // The location of the current/active waypoint.  Used for altitude ramp, track following and loiter calculations.
+    Location next_WP_loc;
+
+    // The location of the active waypoint in Guided mode.
+    struct Location guided_WP_loc;
+
+    // special purpose command used only after mission completed to return vehicle to home or rally point
+    struct AP_Mission::Mission_Command auto_rtl_command;
+
+    // Altitude control
+    struct {
+        // target altitude above sea level in cm. Used for barometric
+        // altitude navigation
+        int32_t amsl_cm;
+
+        // Altitude difference between previous and current waypoint in
+        // centimeters. Used for glide slope handling
+        int32_t offset_cm;
+
+#if AP_TERRAIN_AVAILABLE
+        // are we trying to follow terrain?
+        bool terrain_following;
+
+        // target altitude above terrain in cm, valid if terrain_following
+        // is set
+        int32_t terrain_alt_cm;
+
+        // lookahead value for height error reporting
+        float lookahead;
+#endif
+    } target_altitude;
+
+    // INS variables
+    // The main loop execution time.  Seconds
+    // This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
+    float G_Dt;
+
+    // Performance monitoring
+    // Timer used to accrue data and trigger recording of the performanc monitoring log message
+    uint32_t perf_mon_timer;
+
+    // The maximum and minimum main loop execution time recorded in the current performance monitoring interval
+    uint32_t G_Dt_max;
+    uint32_t G_Dt_min;
+
+    // System Timers
+    // Time in microseconds of start of main control loop
+    uint32_t fast_loopTimer_us;
+
+    // Number of milliseconds used in last main loop cycle
+    uint32_t delta_us_fast_loop;
+
+    // Counter of main loop executions.  Used for performance monitoring and failsafe processing
+    uint16_t mainLoop_count;
+
+    // Camera/Antenna mount tracking and stabilisation stuff
+#if MOUNT == ENABLED
+    // current_loc uses the baro/gps soloution for altitude rather than gps only.
+    AP_Mount camera_mount;
+#endif
+
+    // Arming/Disarming mangement class
+    AP_Arming arming;
+
+    AP_Param param_loader;
+
+    static const AP_Scheduler::Task scheduler_tasks[];
+    static const AP_Param::Info var_info[];
+
+    bool demoing_servos;
+
+    // use this to prevent recursion during sensor init
+    bool in_mavlink_delay;
+
+    // true if we are out of time in our event timeslice
+    bool gcs_out_of_time;
+
+
+    void demo_servos(uint8_t i);
+    void adjust_nav_pitch_throttle(void);
+    void update_load_factor(void);
+    void send_heartbeat(mavlink_channel_t chan);
+    void send_attitude(mavlink_channel_t chan);
+    void send_fence_status(mavlink_channel_t chan);
+    void send_extended_status1(mavlink_channel_t chan);
+    void send_location(mavlink_channel_t chan);
+    void send_nav_controller_output(mavlink_channel_t chan);
+    void send_servo_out(mavlink_channel_t chan);
+    void send_radio_out(mavlink_channel_t chan);
+    void send_vfr_hud(mavlink_channel_t chan);
+    void send_simstate(mavlink_channel_t chan);
+    void send_hwstatus(mavlink_channel_t chan);
+    void send_wind(mavlink_channel_t chan);
+    void send_rangefinder(mavlink_channel_t chan);
+    void send_current_waypoint(mavlink_channel_t chan);
+    void send_statustext(mavlink_channel_t chan);
+    bool telemetry_delayed(mavlink_channel_t chan);
+    void gcs_send_message(enum ap_message id);
+    void gcs_data_stream_send(void);
+    void gcs_update(void);
+    void gcs_send_text_P(gcs_severity severity, const prog_char_t *str);
+    void gcs_send_airspeed_calibration(const Vector3f &vg);
+    void gcs_retry_deferred(void);
+    void do_erase_logs(void);
+    void Log_Write_Attitude(void);
+    void Log_Write_Performance();
+    void Log_Write_Startup(uint8_t type);
+    void Log_Write_EntireMission();
+    void Log_Write_Control_Tuning();
+    void Log_Write_TECS_Tuning(void);
+    void Log_Write_Nav_Tuning();
+    void Log_Write_Status();
+    void Log_Write_Sonar();
+    void Log_Write_Optflow();
+    void Log_Write_Current();
+    void Log_Arm_Disarm();
+    void Log_Write_GPS(uint8_t instance);
+    void Log_Write_IMU();
+    void Log_Write_RC(void);
+    void Log_Write_Baro(void);
+    void Log_Write_Airspeed(void);
+    void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page);
+    void start_logging();
+    void load_parameters(void);
+    void adjust_altitude_target();
+    void setup_glide_slope(void);
+    int32_t get_RTL_altitude();
+    float relative_altitude(void);
+    int32_t relative_altitude_abs_cm(void);
+    void set_target_altitude_current(void);
+    void set_target_altitude_current_adjusted(void);
+    void set_target_altitude_location(const Location &loc);
+    int32_t relative_target_altitude_cm(void);
+    void change_target_altitude(int32_t change_cm);
+    void set_target_altitude_proportion(const Location &loc, float proportion);
+    void constrain_target_altitude_location(const Location &loc1, const Location &loc2);
+    int32_t calc_altitude_error_cm(void);
+    void check_minimum_altitude(void);
+    void reset_offset_altitude(void);
+    void set_offset_altitude_location(const Location &loc);
+    bool above_location_current(const Location &loc);
+    void setup_terrain_target_alt(Location &loc);
+    int32_t adjusted_altitude_cm(void);
+    int32_t adjusted_relative_altitude_cm(void);
+    float height_above_target(void);
+    float lookahead_adjustment(void);
+    float rangefinder_correction(void);
+    void rangefinder_height_update(void);
+    void add_altitude_data(unsigned long xl, long y);
+    void set_next_WP(const struct Location &loc);
+    void set_guided_WP(void);
+    void init_home();
+    void update_home();
+    void do_RTL(void);
+    bool verify_takeoff();
+    bool verify_loiter_unlim();
+    bool verify_loiter_time();
+    bool verify_loiter_turns();
+    bool verify_loiter_to_alt();
+    bool verify_RTL();
+    bool verify_continue_and_change_alt();
+    bool verify_wait_delay();
+    bool verify_change_alt();
+    bool verify_within_distance();
+    void do_loiter_at_location();
+    void do_take_picture();
+    void log_picture();
+    void exit_mission_callback();
+    void update_commands(void);
+    void mavlink_delay(uint32_t ms);
+    void read_control_switch();
+    uint8_t readSwitch(void);
+    void reset_control_switch();
+    void autotune_start(void);
+    void autotune_restore(void);
+    bool fly_inverted(void);
+    void failsafe_short_on_event(enum failsafe_state fstype);
+    void failsafe_long_on_event(enum failsafe_state fstype);
+    void failsafe_short_off_event();
+    void low_battery_event(void);
+    void update_events(void);
+    uint8_t max_fencepoints(void);
+    Vector2l get_fence_point_with_index(unsigned i);
+    void set_fence_point_with_index(Vector2l &point, unsigned i);
+    void geofence_load(void);
+    bool geofence_present(void);
+    void geofence_update_pwm_enabled_state();
+    bool geofence_set_enabled(bool enable, GeofenceEnableReason r);
+    bool geofence_enabled(void);
+    bool geofence_set_floor_enabled(bool floor_enable);
+    bool geofence_check_minalt(void);
+    bool geofence_check_maxalt(void);
+    void geofence_check(bool altitude_check_only);
+    bool geofence_stickmixing(void);
+    void geofence_send_status(mavlink_channel_t chan);
+    bool geofence_breached(void);
+    bool verify_land();
+    void disarm_if_autoland_complete();
+    void setup_landing_glide_slope(void);
+    bool jump_to_landing_sequence(void);
+    float tecs_hgt_afe(void);
+    void set_nav_controller(void);
+    void loiter_angle_reset(void);
+    void loiter_angle_update(void);
+    void navigate();
+    void calc_airspeed_errors();
+    void calc_gndspeed_undershoot();
+    void update_loiter();
+    void update_cruise();
+    void update_fbwb_speed_height(void);
+    void setup_turn_angle(void);
+    bool create_mixer_file(const char *filename);
+    bool setup_failsafe_mixing(void);
+    void set_control_channels(void);
+    void init_rc_in();
+    void init_rc_out();
+    void rudder_arm_check();
+    void read_radio();
+    void control_failsafe(uint16_t pwm);
+    void trim_control_surfaces();
+    void trim_radio();
+    bool rc_failsafe_active(void);
+    void init_barometer(void);
+    void init_rangefinder(void);
+    void read_rangefinder(void);
+    void read_airspeed(void);
+    void zero_airspeed(bool in_startup);
+    void read_battery(void);
+    void read_receiver_rssi(void);
+    void report_radio();
+    void report_ins();
+    void report_compass();
+    void print_radio_values();
+    void print_done();
+    void print_blanks(int16_t num);
+    void print_divider(void);
+    void zero_eeprom(void);
+    void print_enabled(bool b);
+    void print_accel_offsets_and_scaling(void);
+    void print_gyro_offsets(void);
+    void init_ardupilot();
+    void startup_ground(void);
+    enum FlightMode get_previous_mode();
+    void set_mode(enum FlightMode mode);
+    bool mavlink_set_mode(uint8_t mode);
+    void exit_mode(enum FlightMode mode);
+    void check_long_failsafe();
+    void check_short_failsafe();
+    void startup_INS_ground(void);
+    void update_notify();
+    void resetPerfData(void);
+    void check_usb_mux(void);
+    void print_comma(void);
+    void servo_write(uint8_t ch, uint16_t pwm);
+    bool should_log(uint32_t mask);
+    void frsky_telemetry_send(void);
+    uint8_t throttle_percentage(void);
+    void change_arm_state(void);
+    bool disarm_motors(void);
+    bool arm_motors(AP_Arming::ArmingMethod method);
+    bool auto_takeoff_check(void);
+    void takeoff_calc_roll(void);
+    void takeoff_calc_pitch(void);
+    int8_t takeoff_tail_hold(void);
+    void print_hit_enter();
+    void ahrs_update();
+    void update_speed_height(void);
+    void update_GPS_50Hz(void);
+    void update_GPS_10Hz(void);
+    void update_compass(void);
+    void update_alt(void);
+    void obc_fs_check(void);
+    void compass_accumulate(void);
+    void barometer_accumulate(void);
+    void update_optical_flow(void);
+    void one_second_loop(void);
+    void airspeed_ratio_update(void);
+    void update_mount(void);
+    void log_perf_info(void);
+    void compass_save(void);
+    void update_logging1(void);
+    void update_logging2(void);
+    void terrain_update(void);
+    void update_flight_mode(void);
+    void stabilize();
+    void set_servos();
+    void update_aux();
+    void determine_is_flying(void);
+    void gcs_send_text_fmt(const prog_char_t *fmt, ...);
+    void handle_auto_mode(void);
+    void calc_throttle();
+    void calc_nav_roll();
+    void calc_nav_pitch();
+    void update_flight_stage();
+    void update_navigation();
+    void set_flight_stage(AP_SpdHgtControl::FlightStage fs);
+    bool is_flying(void);
+    float get_speed_scaler(void);
+    bool stick_mixing_enabled(void);
+    void stabilize_roll(float speed_scaler);
+    void stabilize_pitch(float speed_scaler);
+    void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
+    void stabilize_stick_mixing_direct();
+    void stabilize_stick_mixing_fbw();
+    void stabilize_yaw(float speed_scaler);
+    void stabilize_training(float speed_scaler);
+    void stabilize_acro(float speed_scaler);
+    void calc_nav_yaw_coordinated(float speed_scaler);
+    void calc_nav_yaw_course(void);
+    void calc_nav_yaw_ground(void);
+    void throttle_slew_limit(int16_t last_throttle);
+    void flap_slew_limit(int8_t &last_value, int8_t &new_value);
+    bool suppress_throttle(void);
+    void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out);
+    void flaperon_update(int8_t flap_percent);
+    bool start_command(const AP_Mission::Mission_Command& cmd);
+    bool verify_command(const AP_Mission::Mission_Command& cmd);
+    void do_takeoff(const AP_Mission::Mission_Command& cmd);
+    void do_nav_wp(const AP_Mission::Mission_Command& cmd);
+    void do_land(const AP_Mission::Mission_Command& cmd);
+    void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd);
+    void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
+    void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
+    void do_loiter_time(const AP_Mission::Mission_Command& cmd);
+    void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
+    void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
+    bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
+    void do_wait_delay(const AP_Mission::Mission_Command& cmd);
+    void do_change_alt(const AP_Mission::Mission_Command& cmd);
+    void do_within_distance(const AP_Mission::Mission_Command& cmd);
+    void do_change_speed(const AP_Mission::Mission_Command& cmd);
+    void do_set_home(const AP_Mission::Mission_Command& cmd);
+    void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
+    void do_digicam_control(const AP_Mission::Mission_Command& cmd);
+    bool start_command_callback(const AP_Mission::Mission_Command &cmd);
+    bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
+    void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd);
+    void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
+    void run_cli(AP_HAL::UARTDriver *port);
+    void log_init();
+
+public:
+    void mavlink_delay_cb();
+    void failsafe_check(void);
+    bool print_log_menu(void);
+    int8_t dump_log(uint8_t argc, const Menu::arg *argv);
+    int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
+    int8_t select_logs(uint8_t argc, const Menu::arg *argv);
+    int8_t process_logs(uint8_t argc, const Menu::arg *argv);
+    int8_t setup_mode(uint8_t argc, const Menu::arg *argv);
+    int8_t setup_factory(uint8_t argc, const Menu::arg *argv);
+    int8_t setup_erase(uint8_t argc, const Menu::arg *argv);
+    int8_t test_mode(uint8_t argc, const Menu::arg *argv);
+    int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
+    int8_t main_menu_help(uint8_t argc, const Menu::arg *argv);
+    int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
+    int8_t test_passthru(uint8_t argc, const Menu::arg *argv);
+    int8_t test_radio(uint8_t argc, const Menu::arg *argv);
+    int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
+    int8_t test_relay(uint8_t argc, const Menu::arg *argv);
+    int8_t test_wp(uint8_t argc, const Menu::arg *argv);
+    void test_wp_print(const AP_Mission::Mission_Command& cmd);
+    int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
+    int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
+    int8_t test_logging(uint8_t argc, const Menu::arg *argv);
+    int8_t test_gps(uint8_t argc, const Menu::arg *argv);
+    int8_t test_ins(uint8_t argc, const Menu::arg *argv);
+    int8_t test_mag(uint8_t argc, const Menu::arg *argv);
+    int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
+    int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
+    int8_t test_shell(uint8_t argc, const Menu::arg *argv);
+};
+
+#define THISFIRMWARE "ArduPlane V3.3.0beta2"
+
+#define MENU_FUNC(func) AP_HAL_CLASSPROC(&plane, &Plane::func)
+
+extern const AP_HAL::HAL& hal;
+extern Plane plane;

ArduPlane/altitude.cpp

@@ -14,6 +14,8 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include "Plane.h"
+
 /*
   altitude handling routines. These cope with both barometric control
   and terrain following control
@@ -22,7 +24,7 @@
 /*
   adjust altitude target depending on mode
  */
-static void adjust_altitude_target()
+void Plane::adjust_altitude_target()
 {
     if (control_mode == FLY_BY_WIRE_B ||
         control_mode == CRUISE) {
@@ -55,7 +57,7 @@ static void adjust_altitude_target()
 /*
   setup for a gradual glide slope to the next waypoint, if appropriate
  */
-static void setup_glide_slope(void)
+void Plane::setup_glide_slope(void)
 {
     // establish the distance we are travelling to the next waypoint,
     // for calculating out rate of change of altitude
@@ -102,7 +104,7 @@ static void setup_glide_slope(void)
 /*
   return RTL altitude as AMSL altitude
  */
-static int32_t get_RTL_altitude()
+int32_t Plane::get_RTL_altitude()
 {
     if (g.RTL_altitude_cm < 0) {
         return current_loc.alt;
@@ -114,7 +116,7 @@ static int32_t get_RTL_altitude()
 /*
   return relative altitude in meters (relative to home)
  */
-static float relative_altitude(void)
+float Plane::relative_altitude(void)
 {
     return (current_loc.alt - home.alt) * 0.01f;
 }
@@ -122,7 +124,7 @@ static float relative_altitude(void)
 /*
   return relative altitude in centimeters, absolute value
  */
-static int32_t relative_altitude_abs_cm(void)
+int32_t Plane::relative_altitude_abs_cm(void)
 {
     return labs(current_loc.alt - home.alt);
 }
@@ -133,7 +135,7 @@ static int32_t relative_altitude_abs_cm(void)
   setting up for altitude hold, such as when releasing elevator in
   CRUISE mode.
  */
-static void set_target_altitude_current(void)
+void Plane::set_target_altitude_current(void)
 {
     // record altitude above sea level at the current time as our
     // target altitude
@@ -160,7 +162,7 @@ static void set_target_altitude_current(void)
 /*
   set the target altitude to the current altitude, with ALT_OFFSET adjustment
  */
-static void set_target_altitude_current_adjusted(void)
+void Plane::set_target_altitude_current_adjusted(void)
 {
     set_target_altitude_current();
 
@@ -171,7 +173,7 @@ static void set_target_altitude_current_adjusted(void)
 /*
   set target altitude based on a location structure
  */
-static void set_target_altitude_location(const Location &loc)
+void Plane::set_target_altitude_location(const Location &loc)
 {
     target_altitude.amsl_cm = loc.alt;
     if (loc.flags.relative_alt) {
@@ -201,7 +203,7 @@ static void set_target_altitude_location(const Location &loc)
   return relative to home target altitude in centimeters. Used for
   altitude control libraries
  */
-static int32_t relative_target_altitude_cm(void)
+int32_t Plane::relative_target_altitude_cm(void)
 {
 #if AP_TERRAIN_AVAILABLE
     float relative_home_height;
@@ -230,7 +232,7 @@ static int32_t relative_target_altitude_cm(void)
   change the current target altitude by an amount in centimeters. Used
   to cope with changes due to elevator in CRUISE or FBWB
  */
-static void change_target_altitude(int32_t change_cm)
+void Plane::change_target_altitude(int32_t change_cm)
 {
     target_altitude.amsl_cm += change_cm;
 #if AP_TERRAIN_AVAILABLE
@@ -251,7 +253,7 @@ static void change_target_altitude(int32_t change_cm)
   Note that target_altitude is setup initially based on the
   destination waypoint
  */
-static void set_target_altitude_proportion(const Location &loc, float proportion)
+void Plane::set_target_altitude_proportion(const Location &loc, float proportion)
 {
     set_target_altitude_location(loc);
     proportion = constrain_float(proportion, 0.0f, 1.0f);
@@ -273,7 +275,7 @@ static void set_target_altitude_proportion(const Location &loc, float proportion
   constrain target altitude to be between two locations. Used to
   ensure we stay within two waypoints in altitude
  */
-static void constrain_target_altitude_location(const Location &loc1, const Location &loc2)
+void Plane::constrain_target_altitude_location(const Location &loc1, const Location &loc2)
 {
     if (loc1.alt > loc2.alt) {
         target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc2.alt, loc1.alt);
@@ -285,7 +287,7 @@ static void constrain_target_altitude_location(const Location &loc1, const Locat
 /*
   return error between target altitude and current altitude
  */
-static int32_t calc_altitude_error_cm(void)
+int32_t Plane::calc_altitude_error_cm(void)
 {
 #if AP_TERRAIN_AVAILABLE
     float terrain_height;
@@ -300,7 +302,7 @@ static int32_t calc_altitude_error_cm(void)
 /*
   check for FBWB_min_altitude_cm violation
  */
-static void check_minimum_altitude(void)
+void Plane::check_minimum_altitude(void)
 {
     if (g.FBWB_min_altitude_cm == 0) {
         return;
@@ -324,7 +326,7 @@ static void check_minimum_altitude(void)
 /*
   reset the altitude offset used for glide slopes
  */
-static void reset_offset_altitude(void)
+void Plane::reset_offset_altitude(void)
 {
     target_altitude.offset_cm = 0;
 }
@@ -336,7 +338,7 @@ static void reset_offset_altitude(void)
   destination is above the current altitude then the result is
   positive.
  */
-static void set_offset_altitude_location(const Location &loc)
+void Plane::set_offset_altitude_location(const Location &loc)
 {
     target_altitude.offset_cm = loc.alt - current_loc.alt;
 
@@ -381,7 +383,7 @@ static void set_offset_altitude_location(const Location &loc)
   lower pressure altitude, if current_loc is in a low part of the
   terrain
  */
-static bool above_location_current(const Location &loc)
+bool Plane::above_location_current(const Location &loc)
 {
 #if AP_TERRAIN_AVAILABLE
     float terrain_alt;
@@ -406,7 +408,7 @@ static bool above_location_current(const Location &loc)
   modify a destination to be setup for terrain following if
   TERRAIN_FOLLOW is enabled
  */
-static void setup_terrain_target_alt(Location &loc)
+void Plane::setup_terrain_target_alt(Location &loc)
 {
 #if AP_TERRAIN_AVAILABLE
     if (g.terrain_follow) {
@@ -420,7 +422,7 @@ static void setup_terrain_target_alt(Location &loc)
   This is useful during long flights to account for barometer changes
   from the GCS, or to adjust the flying height of a long mission
  */
-static int32_t adjusted_altitude_cm(void)
+int32_t Plane::adjusted_altitude_cm(void)
 {
     return current_loc.alt - (g.alt_offset*100);
 }
@@ -430,7 +432,7 @@ static int32_t adjusted_altitude_cm(void)
   during long flights to account for barometer changes from the GCS,
   or to adjust the flying height of a long mission
  */
-static int32_t adjusted_relative_altitude_cm(void)
+int32_t Plane::adjusted_relative_altitude_cm(void)
 {
     return adjusted_altitude_cm() - home.alt;
 }
@@ -438,7 +440,7 @@ static int32_t adjusted_relative_altitude_cm(void)
 /*
   return the height in meters above the next_WP_loc altitude
  */
-static float height_above_target(void)
+float Plane::height_above_target(void)
 {
     float target_alt = next_WP_loc.alt*0.01;
     if (!next_WP_loc.flags.relative_alt) {
@@ -460,7 +462,7 @@ static float height_above_target(void)
 /*
   work out target altitude adjustment from terrain lookahead
  */
-static float lookahead_adjustment(void)
+float Plane::lookahead_adjustment(void)
 {
 #if AP_TERRAIN_AVAILABLE
     int32_t bearing_cd;
@@ -521,7 +523,7 @@ static float lookahead_adjustment(void)
   meters to correct target altitude. A positive number means we need
   to ask the speed/height controller to fly higher
  */
-static float rangefinder_correction(void)
+float Plane::rangefinder_correction(void)
 {
     if (hal.scheduler->millis() - rangefinder_state.last_correction_time_ms > 5000) {
         // we haven't had any rangefinder data for 5s - don't use it
@@ -544,7 +546,7 @@ static float rangefinder_correction(void)
 /*
   update the offset between rangefinder height and terrain height
  */
-static void rangefinder_height_update(void)
+void Plane::rangefinder_height_update(void)
 {
     uint16_t distance_cm = rangefinder.distance_cm();
     float height_estimate = 0;

ArduPlane/climb_rate.cpp

@@ -1,5 +1,7 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Plane.h"
+
 #if 0 // currently unused
 
 struct DataPoint {

ArduPlane/commands.cpp

@@ -3,10 +3,12 @@
  *  logic for dealing with the current command in the mission and home location
  */
 
+#include "Plane.h"
+
 /*
  *  set_next_WP - sets the target location the vehicle should fly to
  */
-static void set_next_WP(const struct Location &loc)
+void Plane::set_next_WP(const struct Location &loc)
 {
     if (auto_state.next_wp_no_crosstrack) {
         // we should not try to cross-track for this waypoint
@@ -67,7 +69,7 @@ static void set_next_WP(const struct Location &loc)
     loiter_angle_reset();
 }
 
-static void set_guided_WP(void)
+void Plane::set_guided_WP(void)
 {
     if (g.loiter_radius < 0) {
         loiter.direction = -1;
@@ -96,7 +98,7 @@ static void set_guided_WP(void)
 
 // run this at setup on the ground
 // -------------------------------
-static void init_home()
+void Plane::init_home()
 {
     gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
 
@@ -118,7 +120,7 @@ static void init_home()
   this is called as long as we have 3D lock and the arming switch is
   not pushed
 */
-static void update_home()
+void Plane::update_home()
 {
     if (home_is_set == HOME_SET_NOT_LOCKED) {
         ahrs.set_home(gps.location());

ArduPlane/commands_logic.cpp

@@ -1,35 +1,11 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-// forward declarations to make compiler happy
+#include "Plane.h"
-static void do_takeoff(const AP_Mission::Mission_Command& cmd);
-static void do_nav_wp(const AP_Mission::Mission_Command& cmd);
-static void do_land(const AP_Mission::Mission_Command& cmd);
-static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
-static void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
-static void do_loiter_time(const AP_Mission::Mission_Command& cmd);
-static void do_wait_delay(const AP_Mission::Mission_Command& cmd);
-static void do_within_distance(const AP_Mission::Mission_Command& cmd);
-static void do_change_alt(const AP_Mission::Mission_Command& cmd);
-static void do_change_speed(const AP_Mission::Mission_Command& cmd);
-static void do_set_home(const AP_Mission::Mission_Command& cmd);
-static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
-static void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
-static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
-#if CAMERA == ENABLED
-static void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
-static void do_digicam_control(const AP_Mission::Mission_Command& cmd);
-#endif
-
 
 /********************************************************************************/
 // Command Event Handlers
 /********************************************************************************/
-
+bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
-/********************************************************************************/
-// Command Event Handlers
-/********************************************************************************/
-static bool
-start_command(const AP_Mission::Mission_Command& cmd)
 {
     // log when new commands start
     if (should_log(MASK_LOG_CMD)) {
@@ -213,7 +189,7 @@ operation returns true when the mission element has completed and we
 should move onto the next mission element.
 *******************************************************************************/
 
-static bool verify_command(const AP_Mission::Mission_Command& cmd)        // Returns true if command complete
+bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Returns true if command complete
 {
     switch(cmd.id) {
 
@@ -293,7 +269,7 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
 //  Nav (Must) commands
 /********************************************************************************/
 
-static void do_RTL(void)
+void Plane::do_RTL(void)
 {
     auto_state.next_wp_no_crosstrack = true;
     auto_state.no_crosstrack = true;
@@ -316,7 +292,7 @@ static void do_RTL(void)
         DataFlash.Log_Write_Mode(control_mode);
 }
 
-static void do_takeoff(const AP_Mission::Mission_Command& cmd)
+void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
 {
     prev_WP_loc = current_loc;
     set_next_WP(cmd.content.location);
@@ -334,18 +310,18 @@ static void do_takeoff(const AP_Mission::Mission_Command& cmd)
     
 }
 
-static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
+void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
     set_next_WP(cmd.content.location);
 }
 
-static void do_land(const AP_Mission::Mission_Command& cmd)
+void Plane::do_land(const AP_Mission::Mission_Command& cmd)
 {
     auto_state.commanded_go_around = false;
     set_next_WP(cmd.content.location);
 }
 
-static void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
+void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.content.location.flags.loiter_ccw) {
         loiter.direction = -1;
@@ -354,20 +330,20 @@ static void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
     }
 }
 
-static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
+void Plane::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
 {
     set_next_WP(cmd.content.location);
     loiter_set_direction_wp(cmd);
 }
 
-static void do_loiter_turns(const AP_Mission::Mission_Command& cmd)
+void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd)
 {
     set_next_WP(cmd.content.location);
     loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL;
     loiter_set_direction_wp(cmd);
 }
 
-static void do_loiter_time(const AP_Mission::Mission_Command& cmd)
+void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd)
 {
     set_next_WP(cmd.content.location);
     // we set start_time_ms when we reach the waypoint
@@ -376,13 +352,13 @@ static void do_loiter_time(const AP_Mission::Mission_Command& cmd)
     loiter_set_direction_wp(cmd);
 }
 
-static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
+void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
 {
     next_WP_loc.alt = cmd.content.location.alt + home.alt;
     reset_offset_altitude();
 }
 
-static void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) 
+void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) 
 {
     //set target alt  
     next_WP_loc.alt = cmd.content.location.alt;
@@ -411,7 +387,7 @@ static void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
 /********************************************************************************/
 //  Verify Nav (Must) commands
 /********************************************************************************/
-static bool verify_takeoff()
+bool Plane::verify_takeoff()
 {
     if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) {
         const float min_gps_speed = 5;
@@ -477,7 +453,7 @@ static bool verify_takeoff()
   update navigation for normal mission waypoints. Return true when the
   waypoint is complete
  */
-static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
+bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
     steer_state.hold_course_cd = -1;
 
@@ -521,28 +497,28 @@ static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
     return false;
 }
 
-static bool verify_loiter_unlim()
+bool Plane::verify_loiter_unlim()
 {
     update_loiter();
     return false;
 }
 
-static bool verify_loiter_time()
+bool Plane::verify_loiter_time()
 {
     update_loiter();
     if (loiter.start_time_ms == 0) {
         if (nav_controller->reached_loiter_target()) {
             // we've reached the target, start the timer
-            loiter.start_time_ms = millis();
+            loiter.start_time_ms = hal.scheduler->millis();
         }
-    } else if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
+    } else if ((hal.scheduler->millis() - loiter.start_time_ms) > loiter.time_max_ms) {
         gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
         return true;
     }
     return false;
 }
 
-static bool verify_loiter_turns()
+bool Plane::verify_loiter_turns()
 {
     update_loiter();
     if (loiter.sum_cd > loiter.total_cd) {
@@ -559,7 +535,7 @@ static bool verify_loiter_turns()
   reached both the desired altitude and desired heading. The desired
   altitude only needs to be reached once.
  */
-static bool verify_loiter_to_alt() 
+bool Plane::verify_loiter_to_alt() 
 {
     update_loiter();
 
@@ -618,7 +594,7 @@ static bool verify_loiter_to_alt()
     return true;
 }
 
-static bool verify_RTL()
+bool Plane::verify_RTL()
 {
     update_loiter();
 	if (auto_state.wp_distance <= (uint32_t)max(g.waypoint_radius,0) || 
@@ -630,7 +606,7 @@ static bool verify_RTL()
 	}
 }
 
-static bool verify_continue_and_change_alt()
+bool Plane::verify_continue_and_change_alt()
 {
     if (abs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
         return true;
@@ -653,16 +629,16 @@ static bool verify_continue_and_change_alt()
 //  Condition (May) commands
 /********************************************************************************/
 
-static void do_wait_delay(const AP_Mission::Mission_Command& cmd)
+void Plane::do_wait_delay(const AP_Mission::Mission_Command& cmd)
 {
-    condition_start = millis();
+    condition_start = hal.scheduler->millis();
     condition_value  = cmd.content.delay.seconds * 1000;    // convert seconds to milliseconds
 }
 
 /*
   process a DO_CHANGE_ALT request
  */
-static void do_change_alt(const AP_Mission::Mission_Command& cmd)
+void Plane::do_change_alt(const AP_Mission::Mission_Command& cmd)
 {
     condition_rate = labs((int)cmd.content.location.lat);   // climb rate in cm/s
     condition_value = cmd.content.location.alt;             // To-Do: ensure this altitude is an absolute altitude?
@@ -676,7 +652,7 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd)
     setup_glide_slope();
 }
 
-static void do_within_distance(const AP_Mission::Mission_Command& cmd)
+void Plane::do_within_distance(const AP_Mission::Mission_Command& cmd)
 {
     condition_value  = cmd.content.distance.meters;
 }
@@ -685,16 +661,16 @@ static void do_within_distance(const AP_Mission::Mission_Command& cmd)
 // Verify Condition (May) commands
 /********************************************************************************/
 
-static bool verify_wait_delay()
+bool Plane::verify_wait_delay()
 {
-    if ((unsigned)(millis() - condition_start) > (unsigned)condition_value) {
+    if ((unsigned)(hal.scheduler->millis() - condition_start) > (unsigned)condition_value) {
         condition_value         = 0;
         return true;
     }
     return false;
 }
 
-static bool verify_change_alt()
+bool Plane::verify_change_alt()
 {
     if( (condition_rate>=0 && adjusted_altitude_cm() >= condition_value) || 
         (condition_rate<=0 && adjusted_altitude_cm() <= condition_value)) {
@@ -707,7 +683,7 @@ static bool verify_change_alt()
     return false;
 }
 
-static bool verify_within_distance()
+bool Plane::verify_within_distance()
 {
     if (auto_state.wp_distance < max(condition_value,0)) {
         condition_value = 0;
@@ -720,7 +696,7 @@ static bool verify_within_distance()
 //  Do (Now) commands
 /********************************************************************************/
 
-static void do_loiter_at_location()
+void Plane::do_loiter_at_location()
 {
     if (g.loiter_radius < 0) {
         loiter.direction = -1;
@@ -730,7 +706,7 @@ static void do_loiter_at_location()
     next_WP_loc = current_loc;
 }
 
-static void do_change_speed(const AP_Mission::Mission_Command& cmd)
+void Plane::do_change_speed(const AP_Mission::Mission_Command& cmd)
 {
     switch (cmd.content.speed.speed_type)
     {
@@ -752,7 +728,7 @@ static void do_change_speed(const AP_Mission::Mission_Command& cmd)
     }
 }
 
-static void do_set_home(const AP_Mission::Mission_Command& cmd)
+void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.p1 == 1 && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
         init_home();
@@ -763,7 +739,7 @@ static void do_set_home(const AP_Mission::Mission_Command& cmd)
 }
 
 // do_digicam_configure Send Digicam Configure message with the camera library
-static void do_digicam_configure(const AP_Mission::Mission_Command& cmd)
+void Plane::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
 {
 #if CAMERA == ENABLED
     camera.configure_cmd(cmd);
@@ -771,7 +747,7 @@ static void do_digicam_configure(const AP_Mission::Mission_Command& cmd)
 }
 
 // do_digicam_control Send Digicam Control message with the camera library
-static void do_digicam_control(const AP_Mission::Mission_Command& cmd)
+void Plane::do_digicam_control(const AP_Mission::Mission_Command& cmd)
 {
 #if CAMERA == ENABLED
     camera.control_cmd(cmd);
@@ -780,7 +756,7 @@ static void do_digicam_control(const AP_Mission::Mission_Command& cmd)
 }
 
 // do_take_picture - take a picture with the camera library
-static void do_take_picture()
+void Plane::do_take_picture()
 {
 #if CAMERA == ENABLED
     camera.trigger_pic(true);
@@ -789,7 +765,7 @@ static void do_take_picture()
 }
 
 // log_picture - log picture taken and send feedback to GCS
-static void log_picture()
+void Plane::log_picture()
 {
     gcs_send_message(MSG_CAMERA_FEEDBACK);
     if (should_log(MASK_LOG_CAMERA)) {
@@ -799,7 +775,7 @@ static void log_picture()
 
 // start_command_callback - callback function called from ap-mission when it begins a new mission command
 //      we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
-static bool start_command_callback(const AP_Mission::Mission_Command &cmd)
+bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd)
 {
     if (control_mode == AUTO) {
         return start_command(cmd);
@@ -809,7 +785,7 @@ static bool start_command_callback(const AP_Mission::Mission_Command &cmd)
 
 // verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
 //      we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
-static bool verify_command_callback(const AP_Mission::Mission_Command& cmd)
+bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
 {
     if (control_mode == AUTO) {
         return verify_command(cmd);
@@ -819,7 +795,7 @@ static bool verify_command_callback(const AP_Mission::Mission_Command& cmd)
 
 // exit_mission_callback - callback function called from ap-mission when the mission has completed
 //      we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
-static void exit_mission_callback()
+void Plane::exit_mission_callback()
 {
     if (control_mode == AUTO) {
         gcs_send_text_fmt(PSTR("Returning to Home"));

ArduPlane/commands_process.cpp

@@ -1,11 +1,10 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-// forward declaration of verify_command to keep compiler happy
+#include "Plane.h"
-static bool verify_command(const AP_Mission::Mission_Command& cmd);
 
 // called by update navigation at 10Hz
 // --------------------
-static void update_commands(void)
+void Plane::update_commands(void)
 {
     if(control_mode == AUTO) {
         if (home_is_set != HOME_UNSET) {

ArduPlane/compat.h

@@ -1,12 +0,0 @@
-
-#ifndef __COMPAT_H__
-#define __COMPAT_H__
-
-#define HIGH 1
-#define LOW 0
-
-/* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */
-static void run_cli(AP_HAL::UARTDriver *port);
-
-#endif // __COMPAT_H__
-

ArduPlane/compat.pde

@@ -1,16 +0,0 @@
-
-static void mavlink_delay(uint32_t ms)
-{
-    hal.scheduler->delay(ms);
-}
-
-static uint32_t millis()
-{
-    return hal.scheduler->millis();
-}
-
-static uint32_t micros()
-{
-    return hal.scheduler->micros();
-}
-

ArduPlane/control_modes.cpp

@@ -1,7 +1,8 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Plane.h"
 
-static void read_control_switch()
+void Plane::read_control_switch()
 {
     static bool switch_debouncer;
     uint8_t switchPosition = readSwitch();
@@ -101,7 +102,7 @@ static void read_control_switch()
 #endif // CONFIG_HAL_BOARD
 }
 
-static uint8_t readSwitch(void)
+uint8_t Plane::readSwitch(void)
 {
     uint16_t pulsewidth = hal.rcin->read(g.flight_mode_channel - 1);
     if (pulsewidth <= 900 || pulsewidth >= 2200) return 255;            // This is an error condition
@@ -113,7 +114,7 @@ static uint8_t readSwitch(void)
     return 0;
 }
 
-static void reset_control_switch()
+void Plane::reset_control_switch()
 {
     oldSwitchPosition = 254;
     read_control_switch();
@@ -122,7 +123,7 @@ static void reset_control_switch()
 /*
   called when entering autotune
  */
-static void autotune_start(void)
+void Plane::autotune_start(void)
 {
     rollController.autotune_start();
     pitchController.autotune_start();
@@ -131,7 +132,7 @@ static void autotune_start(void)
 /*
   called when exiting autotune
  */
-static void autotune_restore(void)
+void Plane::autotune_restore(void)
 {
     rollController.autotune_restore();
     pitchController.autotune_restore();
@@ -140,7 +141,7 @@ static void autotune_restore(void)
 /*
   are we flying inverted?
  */
-static bool fly_inverted(void)
+bool Plane::fly_inverted(void)
 {
     if (g.inverted_flight_ch != 0 && inverted_flight) {
         // controlled with INVERTED_FLIGHT_CH

ArduPlane/events.cpp

@@ -1,11 +1,12 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Plane.h"
 
-static void failsafe_short_on_event(enum failsafe_state fstype)
+void Plane::failsafe_short_on_event(enum failsafe_state fstype)
 {
     // This is how to handle a short loss of control signal failsafe.
     failsafe.state = fstype;
-    failsafe.ch3_timer_ms = millis();
+    failsafe.ch3_timer_ms = hal.scheduler->millis();
     gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
     switch(control_mode)
     {
@@ -48,7 +49,7 @@ static void failsafe_short_on_event(enum failsafe_state fstype)
     gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
 }
 
-static void failsafe_long_on_event(enum failsafe_state fstype)
+void Plane::failsafe_long_on_event(enum failsafe_state fstype)
 {
     // This is how to handle a long loss of control signal failsafe.
     gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, "));
@@ -93,7 +94,7 @@ static void failsafe_long_on_event(enum failsafe_state fstype)
     gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
 }
 
-static void failsafe_short_off_event()
+void Plane::failsafe_short_off_event()
 {
     // We're back in radio contact
     gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off"));
@@ -107,7 +108,7 @@ static void failsafe_short_off_event()
     }
 }
 
-void low_battery_event(void)
+void Plane::low_battery_event(void)
 {
     if (failsafe.low_battery) {
         return;
@@ -120,7 +121,7 @@ void low_battery_event(void)
     AP_Notify::flags.failsafe_battery = true;
 }
 
-static void update_events(void)
+void Plane::update_events(void)
 {
     ServoRelayEvents.update_events();
 }

ArduPlane/failsafe.cpp

@@ -1,4 +1,7 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Plane.h"
+
 /*
  *  failsafe support
  *  Andrew Tridgell, December 2011
@@ -13,7 +16,7 @@
  *  this failsafe_check function is called from the core timer interrupt
  *  at 1kHz.
  */
-void failsafe_check(void)
+void Plane::failsafe_check(void)
 {
     static uint16_t last_mainLoop_count;
     static uint32_t last_timestamp;

ArduPlane/geofence.cpp

@@ -4,6 +4,8 @@
  *  Andrew Tridgell, December 2011
  */
 
+#include "Plane.h"
+
 #if GEOFENCE_ENABLED == ENABLED
 
 #define MIN_GEOFENCE_POINTS 5 // 3 to define a minimal polygon (triangle)
@@ -45,7 +47,7 @@ static const StorageAccess fence_storage(StorageManager::StorageFence);
 /*
   maximum number of fencepoints
  */
-static uint8_t max_fencepoints(void)
+uint8_t Plane::max_fencepoints(void)
 {
     return min(255, fence_storage.size() / sizeof(Vector2l));
 }
@@ -53,7 +55,7 @@ static uint8_t max_fencepoints(void)
 /*
  *  fence boundaries fetch/store
  */
-static Vector2l get_fence_point_with_index(unsigned i)
+Vector2l Plane::get_fence_point_with_index(unsigned i)
 {
     Vector2l ret;
 
@@ -69,7 +71,7 @@ static Vector2l get_fence_point_with_index(unsigned i)
 }
 
 // save a fence point
-static void set_fence_point_with_index(Vector2l &point, unsigned i)
+void Plane::set_fence_point_with_index(Vector2l &point, unsigned i)
 {
     if (i >= (unsigned)g.fence_total.get() || i >= max_fencepoints()) {
         // not allowed
@@ -87,7 +89,7 @@ static void set_fence_point_with_index(Vector2l &point, unsigned i)
 /*
  *  allocate and fill the geofence state structure
  */
-static void geofence_load(void)
+void Plane::geofence_load(void)
 {
     uint8_t i;
 
@@ -147,7 +149,7 @@ failed:
  * return true if a geo-fence has been uploaded and
  * FENCE_ACTION is 1 (not necessarily enabled)
  */
-static bool geofence_present(void)
+bool Plane::geofence_present(void)
 {
     //require at least a return point and a triangle
     //to define a geofence area:
@@ -160,7 +162,7 @@ static bool geofence_present(void)
 /*
   check FENCE_CHANNEL and update the is_pwm_enabled state
  */
-static void geofence_update_pwm_enabled_state() 
+void Plane::geofence_update_pwm_enabled_state() 
 {
     bool is_pwm_enabled;
     if (g.fence_channel == 0) {
@@ -188,7 +190,7 @@ static void geofence_update_pwm_enabled_state()
 }
 
 //return true on success, false on failure
-static bool geofence_set_enabled(bool enable, GeofenceEnableReason r) 
+bool Plane::geofence_set_enabled(bool enable, GeofenceEnableReason r) 
 {
     if (geofence_state == NULL && enable) {
         geofence_load();
@@ -210,7 +212,7 @@ static bool geofence_set_enabled(bool enable, GeofenceEnableReason r)
 /*
  *  return true if geo-fencing is enabled
  */
-static bool geofence_enabled(void)
+bool Plane::geofence_enabled(void)
 {
     geofence_update_pwm_enabled_state();
 
@@ -234,7 +236,7 @@ static bool geofence_enabled(void)
  * Set floor state IF the fence is present.
  * Return false on failure to set floor state.
  */
-static bool geofence_set_floor_enabled(bool floor_enable) {
+bool Plane::geofence_set_floor_enabled(bool floor_enable) {
     if (geofence_state == NULL) {
         return false;
     }
@@ -246,7 +248,7 @@ static bool geofence_set_floor_enabled(bool floor_enable) {
 /*
  *  return true if we have breached the geo-fence minimum altiude
  */
-static bool geofence_check_minalt(void)
+bool Plane::geofence_check_minalt(void)
 {
     if (g.fence_maxalt <= g.fence_minalt) {
         return false;
@@ -260,7 +262,7 @@ static bool geofence_check_minalt(void)
 /*
  *  return true if we have breached the geo-fence maximum altiude
  */
-static bool geofence_check_maxalt(void)
+bool Plane::geofence_check_maxalt(void)
 {
     if (g.fence_maxalt <= g.fence_minalt) {
         return false;
@@ -275,7 +277,7 @@ static bool geofence_check_maxalt(void)
 /*
  *  check if we have breached the geo-fence
  */
-static void geofence_check(bool altitude_check_only)
+void Plane::geofence_check(bool altitude_check_only)
 {
     if (!geofence_enabled()) {
         // switch back to the chosen control mode if still in
@@ -354,7 +356,7 @@ static void geofence_check(bool altitude_check_only)
     // we are outside, and have not previously triggered.
     geofence_state->fence_triggered = true;
     geofence_state->breach_count++;
-    geofence_state->breach_time = millis();
+    geofence_state->breach_time = hal.scheduler->millis();
     geofence_state->breach_type = breach_type;
 
  #if FENCE_TRIGGERED_PIN > 0
@@ -420,7 +422,7 @@ static void geofence_check(bool altitude_check_only)
  *  disabled. Otherwise the aircraft may not be able to recover from
  *  a breach of the geo-fence
  */
-static bool geofence_stickmixing(void) {
+bool Plane::geofence_stickmixing(void) {
     if (geofence_enabled() &&
         geofence_state != NULL &&
         geofence_state->fence_triggered &&
@@ -435,7 +437,7 @@ static bool geofence_stickmixing(void) {
 /*
  *
  */
-static void geofence_send_status(mavlink_channel_t chan)
+void Plane::geofence_send_status(mavlink_channel_t chan)
 {
     if (geofence_enabled() && geofence_state != NULL) {
         mavlink_msg_fence_status_send(chan,
@@ -449,7 +451,7 @@ static void geofence_send_status(mavlink_channel_t chan)
 /*
   return true if geofence has been breached
  */
-static bool geofence_breached(void)
+bool Plane::geofence_breached(void)
 {
     return geofence_state ? geofence_state->fence_triggered : false;
 }
@@ -457,24 +459,24 @@ static bool geofence_breached(void)
 
 #else // GEOFENCE_ENABLED
 
-static void geofence_check(bool altitude_check_only) {
+void Plane::geofence_check(bool altitude_check_only) {
 }
-static bool geofence_stickmixing(void) {
+bool Plane::geofence_stickmixing(void) {
     return true;
 }
-static bool geofence_enabled(void) {
+bool Plane::geofence_enabled(void) {
     return false;
 }
 
-static bool geofence_present(void) {
+bool Plane::geofence_present(void) {
     return false;
 }
 
-static bool geofence_set_enabled(bool enable, GeofenceEnableReason r) {
+bool Plane::geofence_set_enabled(bool enable, GeofenceEnableReason r) {
     return false;
 }
 
-static bool geofence_set_floor_enabled(bool floor_enable) {
+bool Plane::geofence_set_floor_enabled(bool floor_enable) {
     return false;
 }
 

ArduPlane/landing.cpp

@@ -1,5 +1,7 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Plane.h"
+
 /*
   landing logic
  */
@@ -8,7 +10,7 @@
   update navigation for landing. Called when on landing approach or
   final flare
  */
-static bool verify_land()
+bool Plane::verify_land()
 {
     // we don't 'verify' landing in the sense that it never completes,
     // so we don't verify command completion. Instead we use this to
@@ -102,7 +104,7 @@ static bool verify_land()
 /*
     If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
  */
-static void disarm_if_autoland_complete()
+void Plane::disarm_if_autoland_complete()
 {
     if (g.land_disarm_delay > 0 && 
         auto_state.land_complete && 
@@ -128,7 +130,7 @@ static void disarm_if_autoland_complete()
   itself as that leads to discontinuities close to the landing point,
   which can lead to erratic pitch control
  */
-static void setup_landing_glide_slope(void)
+void Plane::setup_landing_glide_slope(void)
 {
         Location loc = next_WP_loc;
 
@@ -209,7 +211,7 @@ static void setup_landing_glide_slope(void)
    switch to that mission item.  Returns false if no DO_LAND_START
    available.
  */
-static bool jump_to_landing_sequence(void) 
+bool Plane::jump_to_landing_sequence(void) 
 {
     uint16_t land_idx = mission.get_landing_sequence_start();
     if (land_idx != 0) {
@@ -233,7 +235,7 @@ static bool jump_to_landing_sequence(void)
 /*
   the height above field elevation that we pass to TECS
  */
-static float tecs_hgt_afe(void)
+float Plane::tecs_hgt_afe(void)
 {
     /*
       pass the height above field elevation as the height above

ArduPlane/make.inc

@@ -0,0 +1,56 @@
+LIBRARIES = AP_HAL
+LIBRARIES += AP_Common
+LIBRARIES += AP_Progmem
+LIBRARIES += AP_Menu
+LIBRARIES += AP_Param
+LIBRARIES += StorageManager
+LIBRARIES += AP_GPS
+LIBRARIES += AP_Baro
+LIBRARIES += AP_Compass
+LIBRARIES += AP_Math
+LIBRARIES += AP_ADC
+LIBRARIES += AP_ADC_AnalogSource
+LIBRARIES += AP_InertialSensor
+LIBRARIES += AP_AHRS
+LIBRARIES += RC_Channel
+LIBRARIES += AP_RangeFinder
+LIBRARIES += Filter
+LIBRARIES += AP_Buffer
+LIBRARIES += AP_Relay
+LIBRARIES += AP_Camera
+LIBRARIES += AP_Airspeed
+LIBRARIES += AP_Terrain
+LIBRARIES += APM_OBC
+LIBRARIES += APM_Control
+LIBRARIES += AP_AutoTune
+LIBRARIES += GCS
+LIBRARIES += GCS_MAVLink
+LIBRARIES += AP_SerialManager
+LIBRARIES += AP_Mount
+LIBRARIES += AP_Declination
+LIBRARIES += DataFlash
+LIBRARIES += SITL
+LIBRARIES += AP_Scheduler
+LIBRARIES += AP_Navigation
+LIBRARIES += AP_L1_Control
+LIBRARIES += AP_RCMapper
+LIBRARIES += AP_Vehicle
+LIBRARIES += AP_SpdHgtControl
+LIBRARIES += AP_TECS
+LIBRARIES += AP_NavEKF
+LIBRARIES += AP_Mission
+LIBRARIES += AP_Notify
+LIBRARIES += AP_BattMonitor
+LIBRARIES += AP_Arming
+LIBRARIES += AP_BoardConfig
+LIBRARIES += AP_Frsky_Telem
+LIBRARIES += AP_ServoRelayEvents
+LIBRARIES += AP_Rally
+LIBRARIES += AP_OpticalFlow
+LIBRARIES += AP_HAL_AVR
+LIBRARIES += AP_HAL_SITL
+LIBRARIES += AP_HAL_PX4
+LIBRARIES += AP_HAL_FLYMAPLE
+LIBRARIES += AP_HAL_Linux
+LIBRARIES += AP_HAL_Empty
+LIBRARIES += AP_HAL_VRBRAIN

ArduPlane/navigation.cpp

@@ -1,8 +1,9 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Plane.h"
 
 // set the nav_controller pointer to the right controller
-static void set_nav_controller(void)
+void Plane::set_nav_controller(void)
 {
     switch ((AP_Navigation::ControllerType)g.nav_controller.get()) {
     case AP_Navigation::CONTROLLER_L1:
@@ -14,7 +15,7 @@ static void set_nav_controller(void)
 /*
   reset the total loiter angle
  */
-static void loiter_angle_reset(void)
+void Plane::loiter_angle_reset(void)
 {
     loiter.sum_cd = 0;
     loiter.total_cd = 0;
@@ -24,7 +25,7 @@ static void loiter_angle_reset(void)
   update the total angle we have covered in a loiter. Used to support
   commands to do N circles of loiter
  */
-static void loiter_angle_update(void)
+void Plane::loiter_angle_update(void)
 {
     int32_t target_bearing_cd = nav_controller->target_bearing_cd();
     int32_t loiter_delta_cd;
@@ -43,7 +44,7 @@ static void loiter_angle_update(void)
 //****************************************************************
 // Function that will calculate the desired direction to fly and distance
 //****************************************************************
-static void navigate()
+void Plane::navigate()
 {
     // allow change of nav controller mid-flight
     set_nav_controller();
@@ -72,7 +73,7 @@ static void navigate()
     update_navigation();
 }
 
-static void calc_airspeed_errors()
+void Plane::calc_airspeed_errors()
 {
     float aspeed_cm = airspeed.get_airspeed_cm();
 
@@ -111,7 +112,7 @@ static void calc_airspeed_errors()
     airspeed_error_cm = SpdHgt_Controller->get_target_airspeed()*100 - aspeed_cm;
 }
 
-static void calc_gndspeed_undershoot()
+void Plane::calc_gndspeed_undershoot()
 {
  	// Use the component of ground speed in the forward direction
 	// This prevents flyaway if wind takes plane backwards
@@ -125,7 +126,7 @@ static void calc_gndspeed_undershoot()
     }
 }
 
-static void update_loiter()
+void Plane::update_loiter()
 {
     nav_controller->update_loiter(next_WP_loc, abs(g.loiter_radius), loiter.direction);
 }
@@ -134,7 +135,7 @@ static void update_loiter()
   handle CRUISE mode, locking heading to GPS course when we have
   sufficient ground speed, and no aileron or rudder input
  */
-static void update_cruise()
+void Plane::update_cruise()
 {
     if (!cruise_state.locked_heading &&
         channel_roll->control_in == 0 &&
@@ -170,7 +171,7 @@ static void update_cruise()
   In this mode the elevator is used to change target altitude. The
   throttle is used to change target airspeed or throttle
  */
-static void update_fbwb_speed_height(void)
+void Plane::update_fbwb_speed_height(void)
 {
     static float last_elevator_input;
     float elevator_input;
@@ -202,7 +203,7 @@ static void update_fbwb_speed_height(void)
 /*
   calculate the turn angle for the next leg of the mission
  */
-static void setup_turn_angle(void)
+void Plane::setup_turn_angle(void)
 {
     int32_t next_ground_course_cd = mission.get_next_ground_course_cd(-1);
     if (next_ground_course_cd == -1) {

ArduPlane/px4_mixer.cpp

@@ -1,5 +1,7 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Plane.h"
+
 /*
   handle creation of PX4 mixer file, for failover to direct RC control
   on failure of FMU
@@ -23,7 +25,7 @@
 /*
   create a mixer file given key fixed wing parameters
  */
-static bool create_mixer_file(const char *filename)
+bool Plane::create_mixer_file(const char *filename)
 {
     int mix_fd = open(filename, O_WRONLY|O_CREAT|O_TRUNC, 0644);
     if (mix_fd == -1) {
@@ -185,7 +187,7 @@ static bool create_mixer_file(const char *filename)
 /*
   setup mixer on PX4 so that if FMU dies the pilot gets manual control
  */
-static bool setup_failsafe_mixing(void)
+bool Plane::setup_failsafe_mixing(void)
 {
     // we create MIXER.MIX regardless of whether we will be using it,
     // as it gives a template for the user to modify to create their

ArduPlane/radio.cpp

@@ -1,12 +1,14 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Plane.h"
+
 //Function that will read the radio data, limit servos and trigger a failsafe
 // ----------------------------------------------------------------------------
 
 /*
   allow for runtime change of control channel ordering
  */
-static void set_control_channels(void)
+void Plane::set_control_channels(void)
 {
     if (g.rudder_only) {
         // in rudder only mode the roll and rudder channels are the
@@ -37,7 +39,7 @@ static void set_control_channels(void)
 /*
   initialise RC input channels
  */
-static void init_rc_in()
+void Plane::init_rc_in()
 {
     // set rc dead zones
     channel_roll->set_default_dead_zone(30);
@@ -51,7 +53,7 @@ static void init_rc_in()
 /*
   initialise RC output channels
  */
-static void init_rc_out()
+void Plane::init_rc_out()
 {
     channel_roll->enable_out();
     channel_pitch->enable_out();
@@ -75,7 +77,7 @@ static void init_rc_out()
 }
 
 // check for pilot input on rudder stick for arming
-static void rudder_arm_check() 
+void Plane::rudder_arm_check() 
 {
     //TODO: ensure rudder arming disallowed during radio calibration
 
@@ -108,7 +110,7 @@ static void rudder_arm_check()
 
     // full right rudder starts arming counter
     if (channel_rudder->control_in > 4000) {
-        uint32_t now = millis();
+        uint32_t now = hal.scheduler->millis();
 
         if (rudder_arm_timer == 0 || 
             now - rudder_arm_timer < 3000) {
@@ -124,7 +126,7 @@ static void rudder_arm_check()
     }
 }
 
-static void read_radio()
+void Plane::read_radio()
 {
     if (!hal.rcin->new_input()) {
         control_failsafe(channel_throttle->radio_in);
@@ -186,7 +188,7 @@ static void read_radio()
     }
 }
 
-static void control_failsafe(uint16_t pwm)
+void Plane::control_failsafe(uint16_t pwm)
 {
     if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) {
         // we do not have valid RC input. Set all primary channel
@@ -238,7 +240,7 @@ static void control_failsafe(uint16_t pwm)
     }
 }
 
-static void trim_control_surfaces()
+void Plane::trim_control_surfaces()
 {
     read_radio();
     int16_t trim_roll_range = (channel_roll->radio_max - channel_roll->radio_min)/5;
@@ -292,7 +294,7 @@ static void trim_control_surfaces()
     channel_rudder->save_eeprom();
 }
 
-static void trim_radio()
+void Plane::trim_radio()
 {
     for (uint8_t y = 0; y < 30; y++) {
         read_radio();
@@ -305,7 +307,7 @@ static void trim_radio()
   return true if throttle level is below throttle failsafe threshold
   or RC input is invalid
  */
-static bool rc_failsafe_active(void)
+bool Plane::rc_failsafe_active(void)
 {
     if (!g.throttle_fs_enabled) {
         return false;

ArduPlane/sensors.cpp

@@ -1,6 +1,8 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-static void init_barometer(void)
+#include "Plane.h"
+
+void Plane::init_barometer(void)
 {
     gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));    
     barometer.calibrate();
@@ -8,7 +10,7 @@ static void init_barometer(void)
     gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
 }
 
-static void init_rangefinder(void)
+void Plane::init_rangefinder(void)
 {
     rangefinder.init();
 }
@@ -16,7 +18,7 @@ static void init_rangefinder(void)
 /*
   read the rangefinder and update height estimate
  */
-static void read_rangefinder(void)
+void Plane::read_rangefinder(void)
 {
     rangefinder.update();
 
@@ -29,7 +31,7 @@ static void read_rangefinder(void)
 /*
   ask airspeed sensor for a new value
  */
-static void read_airspeed(void)
+void Plane::read_airspeed(void)
 {
     if (airspeed.enabled()) {
         airspeed.read();
@@ -53,7 +55,7 @@ static void read_airspeed(void)
     }
 }
 
-static void zero_airspeed(bool in_startup)
+void Plane::zero_airspeed(bool in_startup)
 {
     airspeed.calibrate(in_startup);
     read_airspeed();
@@ -64,7 +66,7 @@ static void zero_airspeed(bool in_startup)
 
 // read_battery - reads battery voltage and current and invokes failsafe
 // should be called at 10hz
-static void read_battery(void)
+void Plane::read_battery(void)
 {
     battery.read();
     compass.set_current(battery.current_amps());
@@ -77,7 +79,7 @@ static void read_battery(void)
 
 // read the receiver RSSI as an 8 bit number for MAVLink
 // RC_CHANNELS_SCALED message
-void read_receiver_rssi(void)
+void Plane::read_receiver_rssi(void)
 {
     // avoid divide by zero
     if (g.rssi_range <= 0) {

ArduPlane/setup.cpp

@@ -0,0 +1,81 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Plane.h"
+
+#if CLI_ENABLED == ENABLED
+
+// Command/function table for the setup menu
+static const struct Menu::command setup_menu_commands[] PROGMEM = {
+    // command			function called
+    // =======          ===============
+    {"reset",           MENU_FUNC(setup_factory)},
+    {"erase",           MENU_FUNC(setup_erase)}
+};
+
+// Create the setup menu object.
+MENU(setup_menu, "setup", setup_menu_commands);
+
+// Called from the top-level menu to run the setup menu.
+int8_t Plane::setup_mode(uint8_t argc, const Menu::arg *argv)
+{
+    // Give the user some guidance
+    cliSerial->printf_P(PSTR("Setup Mode\n"
+                         "\n"
+                         "IMPORTANT: if you have not previously set this system up, use the\n"
+                         "'reset' command to initialize the EEPROM to sensible default values\n"
+                         "and then the 'radio' command to configure for your radio.\n"
+                         "\n"));
+
+    // Run the setup menu.  When the menu exits, we will return to the main menu.
+    setup_menu.run();
+    return 0;
+}
+
+// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
+// Called by the setup menu 'factoryreset' command.
+int8_t Plane::setup_factory(uint8_t argc, const Menu::arg *argv)
+{
+    int c;
+
+    cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
+
+    do {
+        c = cliSerial->read();
+    } while (-1 == c);
+
+    if (('y' != c) && ('Y' != c))
+        return(-1);
+    AP_Param::erase_all();
+    cliSerial->printf_P(PSTR("\nFACTORY RESET complete - please reset board to continue"));
+
+    for (;; ) {
+    }
+    // note, cannot actually return here
+    return(0);
+}
+
+
+int8_t Plane::setup_erase(uint8_t argc, const Menu::arg *argv)
+{
+    int c;
+
+    cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
+
+    do {
+        c = cliSerial->read();
+    } while (-1 == c);
+
+    if (('y' != c) && ('Y' != c))
+        return(-1);
+    zero_eeprom();
+    return 0;
+}
+
+void Plane::zero_eeprom(void)
+{
+    cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
+    StorageManager::erase();
+    cliSerial->printf_P(PSTR("done\n"));
+}
+
+#endif // CLI_ENABLED

ArduPlane/setup.pde

@@ -1,441 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-#if CLI_ENABLED == ENABLED
-
-// Functions called from the setup menu
-static int8_t   setup_radio                             (uint8_t argc, const Menu::arg *argv);
-static int8_t   setup_show                              (uint8_t argc, const Menu::arg *argv);
-static int8_t   setup_factory                   (uint8_t argc, const Menu::arg *argv);
-static int8_t   setup_accel_scale                       (uint8_t argc, const Menu::arg *argv);
-static int8_t   setup_set                               (uint8_t argc, const Menu::arg *argv);
-static int8_t   setup_erase                             (uint8_t argc, const Menu::arg *argv);
-static int8_t   setup_compass                   (uint8_t argc, const Menu::arg *argv);
-
-
-// Command/function table for the setup menu
-static const struct Menu::command setup_menu_commands[] PROGMEM = {
-    // command			function called
-    // =======          ===============
-    {"reset",                       setup_factory},
-    {"radio",                       setup_radio},
-    {"accel",                       setup_accel_scale},
-    {"compass",                     setup_compass},
-    {"show",                        setup_show},
-    {"set",                         setup_set},
-    {"erase",                       setup_erase},
-};
-
-// Create the setup menu object.
-MENU(setup_menu, "setup", setup_menu_commands);
-
-// Called from the top-level menu to run the setup menu.
-static int8_t
-setup_mode(uint8_t argc, const Menu::arg *argv)
-{
-    // Give the user some guidance
-    cliSerial->printf_P(PSTR("Setup Mode\n"
-                         "\n"
-                         "IMPORTANT: if you have not previously set this system up, use the\n"
-                         "'reset' command to initialize the EEPROM to sensible default values\n"
-                         "and then the 'radio' command to configure for your radio.\n"
-                         "\n"));
-
-    // Run the setup menu.  When the menu exits, we will return to the main menu.
-    setup_menu.run();
-    return 0;
-}
-
-// Print the current configuration.
-// Called by the setup menu 'show' command.
-static int8_t
-setup_show(uint8_t argc, const Menu::arg *argv)
-{
-    AP_Param *param;
-    ap_var_type type;
-
-    //If a parameter name is given as an argument to show, print only that parameter
-    if(argc>=2)
-    {
-
-        param=AP_Param::find(argv[1].str, &type);
-
-        if(!param)
-        {
-            cliSerial->printf_P(PSTR("Parameter not found: '%s'\n"), argv[1]);
-            return 0;
-        }
-        AP_Param::show(param, argv[1].str, type, cliSerial);
-        return 0;
-    }
-
-    AP_Param::show_all(cliSerial);
-
-    return(0);
-}
-
-//Set a parameter to a specified value. It will cast the value to the current type of the
-//parameter and make sure it fits in case of INT8 and INT16
-static int8_t setup_set(uint8_t argc, const Menu::arg *argv)
-{
-    int8_t value_int8;
-    int16_t value_int16;
-
-    AP_Param *param;
-    enum ap_var_type p_type;
-
-    if(argc!=3)
-    {
-        cliSerial->printf_P(PSTR("Invalid command. Usage: set <name> <value>\n"));
-        return 0;
-    }
-
-    param = AP_Param::find(argv[1].str, &p_type);
-    if(!param)
-    {
-        cliSerial->printf_P(PSTR("Param not found: %s\n"), argv[1].str);
-        return 0;
-    }
-
-    switch(p_type)
-    {
-        case AP_PARAM_INT8:
-            value_int8 = (int8_t)(argv[2].i);
-            if(argv[2].i!=value_int8)
-            {
-                cliSerial->printf_P(PSTR("Value out of range for type INT8\n"));
-                return 0;
-            }
-            ((AP_Int8*)param)->set_and_save(value_int8);
-            break;
-        case AP_PARAM_INT16:
-            value_int16 = (int16_t)(argv[2].i);
-            if(argv[2].i!=value_int16)
-            {
-                cliSerial->printf_P(PSTR("Value out of range for type INT16\n"));
-                return 0;
-            }
-            ((AP_Int16*)param)->set_and_save(value_int16);
-            break;
-
-        //int32 and float don't need bounds checking, just use the value provoded by Menu::arg
-        case AP_PARAM_INT32:
-            ((AP_Int32*)param)->set_and_save(argv[2].i);
-            break;
-        case AP_PARAM_FLOAT:
-            ((AP_Float*)param)->set_and_save(argv[2].f);
-            break;
-        default:
-            cliSerial->printf_P(PSTR("Cannot set parameter of type %d.\n"), p_type);
-            break;
-    }
-
-    return 0;
-}
-
-// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
-// Called by the setup menu 'factoryreset' command.
-static int8_t
-setup_factory(uint8_t argc, const Menu::arg *argv)
-{
-    int c;
-
-    cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
-
-    do {
-        c = cliSerial->read();
-    } while (-1 == c);
-
-    if (('y' != c) && ('Y' != c))
-        return(-1);
-    AP_Param::erase_all();
-    cliSerial->printf_P(PSTR("\nFACTORY RESET complete - please reset board to continue"));
-
-    //default_flight_modes();   // This will not work here.  Replacement code located in init_ardupilot()
-
-    for (;; ) {
-    }
-    // note, cannot actually return here
-    return(0);
-}
-
-
-// Perform radio setup.
-// Called by the setup menu 'radio' command.
-static int8_t
-setup_radio(uint8_t argc, const Menu::arg *argv)
-{
-    cliSerial->printf_P(PSTR("\n\nRadio Setup:\n"));
-    uint8_t i;
-
-    for(i = 0; i < 100; i++) {
-        hal.scheduler->delay(20);
-        read_radio();
-    }
-
-
-    if(channel_roll->radio_in < 500) {
-        while(1) {
-            cliSerial->printf_P(PSTR("\nNo radio; Check connectors."));
-            hal.scheduler->delay(1000);
-            // stop here
-        }
-    }
-
-    channel_roll->radio_min                = channel_roll->radio_in;
-    channel_pitch->radio_min               = channel_pitch->radio_in;
-    channel_throttle->radio_min    = channel_throttle->radio_in;
-    channel_rudder->radio_min              = channel_rudder->radio_in;
-    g.rc_5.radio_min = g.rc_5.radio_in;
-    g.rc_6.radio_min = g.rc_6.radio_in;
-    g.rc_7.radio_min = g.rc_7.radio_in;
-    g.rc_8.radio_min = g.rc_8.radio_in;
-
-    channel_roll->radio_max                = channel_roll->radio_in;
-    channel_pitch->radio_max               = channel_pitch->radio_in;
-    channel_throttle->radio_max    = channel_throttle->radio_in;
-    channel_rudder->radio_max              = channel_rudder->radio_in;
-    g.rc_5.radio_max = g.rc_5.radio_in;
-    g.rc_6.radio_max = g.rc_6.radio_in;
-    g.rc_7.radio_max = g.rc_7.radio_in;
-    g.rc_8.radio_max = g.rc_8.radio_in;
-
-    channel_roll->radio_trim               = channel_roll->radio_in;
-    channel_pitch->radio_trim              = channel_pitch->radio_in;
-    channel_rudder->radio_trim     = channel_rudder->radio_in;
-    g.rc_5.radio_trim = 1500;
-    g.rc_6.radio_trim = 1500;
-    g.rc_7.radio_trim = 1500;
-    g.rc_8.radio_trim = 1500;
-
-    cliSerial->printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
-    while(1) {
-
-        hal.scheduler->delay(20);
-        // Filters radio input - adjust filters in the radio.pde file
-        // ----------------------------------------------------------
-        read_radio();
-
-        channel_roll->update_min_max();
-        channel_pitch->update_min_max();
-        channel_throttle->update_min_max();
-        channel_rudder->update_min_max();
-        g.rc_5.update_min_max();
-        g.rc_6.update_min_max();
-        g.rc_7.update_min_max();
-        g.rc_8.update_min_max();
-
-        if(cliSerial->available() > 0) {
-            while (cliSerial->available() > 0) {
-                cliSerial->read();
-            }
-            channel_roll->save_eeprom();
-            channel_pitch->save_eeprom();
-            channel_throttle->save_eeprom();
-            channel_rudder->save_eeprom();
-            g.rc_5.save_eeprom();
-            g.rc_6.save_eeprom();
-            g.rc_7.save_eeprom();
-            g.rc_8.save_eeprom();
-            print_done();
-            break;
-        }
-    }
-    trim_radio();
-    report_radio();
-    return(0);
-}
-
-
-static int8_t
-setup_erase(uint8_t argc, const Menu::arg *argv)
-{
-    int c;
-
-    cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
-
-    do {
-        c = cliSerial->read();
-    } while (-1 == c);
-
-    if (('y' != c) && ('Y' != c))
-        return(-1);
-    zero_eeprom();
-    return 0;
-}
-
-/*
-  handle full accelerometer calibration via user dialog
- */
-
-static int8_t
-setup_accel_scale(uint8_t argc, const Menu::arg *argv)
-{
-    float trim_roll, trim_pitch;
-    cliSerial->println_P(PSTR("Initialising gyros"));
-
-    ahrs.init();
-    ahrs.set_fly_forward(true);
-    ahrs.set_wind_estimation(true);
-
-    ins.init(AP_InertialSensor::COLD_START, ins_sample_rate);
-    AP_InertialSensor_UserInteractStream interact(hal.console);
-    bool success = ins.calibrate_accel(&interact, trim_roll, trim_pitch);
-    if (success) {
-        // reset ahrs's trim to suggested values from calibration routine
-        ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
-    }
-    report_ins();
-    return(0);
-}
-
-static int8_t
-setup_compass(uint8_t argc, const Menu::arg *argv)
-{
-    if (!strcmp_P(argv[1].str, PSTR("on"))) {
-        if (!compass.init()) {
-            cliSerial->println_P(PSTR("Compass initialisation failed!"));
-            g.compass_enabled = false;
-        } else {
-            g.compass_enabled = true;
-        }
-    } else if (!strcmp_P(argv[1].str, PSTR("off"))) {
-        g.compass_enabled = false;
-
-    } else if (!strcmp_P(argv[1].str, PSTR("reset"))) {
-        compass.set_and_save_offsets(0,0,0,0);
-
-    } else {
-        cliSerial->printf_P(PSTR("\nOptions:[on,off,reset]\n"));
-        report_compass();
-        return 0;
-    }
-
-    g.compass_enabled.save();
-    report_compass();
-    return 0;
-}
-
-/***************************************************************************/
-// CLI reports
-/***************************************************************************/
-
-static void report_radio()
-{
-    //print_blanks(2);
-    cliSerial->printf_P(PSTR("Radio\n"));
-    print_divider();
-    // radio
-    print_radio_values();
-    print_blanks(2);
-}
-
-static void report_ins()
-{
-    //print_blanks(2);
-    cliSerial->printf_P(PSTR("INS\n"));
-    print_divider();
-
-    print_gyro_offsets();
-    print_accel_offsets_and_scaling();
-    print_blanks(2);
-}
-
-static void report_compass()
-{
-    cliSerial->print_P(PSTR("Compass: "));
-    print_enabled(g.compass_enabled);
-
-    Vector3f offsets = compass.get_offsets();
-
-    // mag offsets
-    cliSerial->printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
-                    (double)offsets.x,
-                    (double)offsets.y,
-                    (double)offsets.z);
-    print_blanks(2);
-}
-
-/***************************************************************************/
-// CLI utilities
-/***************************************************************************/
-
-static void
-print_radio_values()
-{
-    cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)channel_roll->radio_min, (int)channel_roll->radio_trim, (int)channel_roll->radio_max);
-    cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)channel_pitch->radio_min, (int)channel_pitch->radio_trim, (int)channel_pitch->radio_max);
-    cliSerial->printf_P(PSTR("CH3: %d | %d | %d\n"), (int)channel_throttle->radio_min, (int)channel_throttle->radio_trim, (int)channel_throttle->radio_max);
-    cliSerial->printf_P(PSTR("CH4: %d | %d | %d\n"), (int)channel_rudder->radio_min, (int)channel_rudder->radio_trim, (int)channel_rudder->radio_max);
-    cliSerial->printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
-    cliSerial->printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
-    cliSerial->printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
-    cliSerial->printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
-
-}
-
-static void
-print_done()
-{
-    cliSerial->printf_P(PSTR("\nSaved Settings\n\n"));
-}
-
-static void
-print_blanks(int16_t num)
-{
-    while(num > 0) {
-        num--;
-        cliSerial->println();
-    }
-}
-
-static void
-print_divider(void)
-{
-    for (int16_t i = 0; i < 40; i++) {
-        cliSerial->printf_P(PSTR("-"));
-    }
-    cliSerial->println();
-}
-
-static void zero_eeprom(void)
-{
-    cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
-    StorageManager::erase();
-    cliSerial->printf_P(PSTR("done\n"));
-}
-
-static void print_enabled(bool b)
-{
-    if(b)
-        cliSerial->printf_P(PSTR("en"));
-    else
-        cliSerial->printf_P(PSTR("dis"));
-    cliSerial->printf_P(PSTR("abled\n"));
-}
-
-static void
-print_accel_offsets_and_scaling(void)
-{
-    Vector3f accel_offsets = ins.get_accel_offsets();
-    Vector3f accel_scale = ins.get_accel_scale();
-    cliSerial->printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"),
-                    (double)accel_offsets.x,                           // Pitch
-                    (double)accel_offsets.y,                           // Roll
-                    (double)accel_offsets.z,                           // YAW
-                    (double)accel_scale.x,                             // Pitch
-                    (double)accel_scale.y,                             // Roll
-                    (double)accel_scale.z);                            // YAW
-}
-
-static void
-print_gyro_offsets(void)
-{
-    Vector3f gyro_offsets = ins.get_gyro_offsets();
-    cliSerial->printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
-                    (double)gyro_offsets.x,
-                    (double)gyro_offsets.y,
-                    (double)gyro_offsets.z);
-}
-
-
-#endif // CLI_ENABLED

ArduPlane/system.cpp

@@ -1,4 +1,7 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "Plane.h"
+
 /*****************************************************************************
 *   The init_ardupilot function processes everything we need for an in - air restart
 *        We will determine later if we are actually on the ground and process a
@@ -8,16 +11,10 @@
 
 #if CLI_ENABLED == ENABLED
 
-// Functions called from the top-level menu
-static int8_t   process_logs(uint8_t argc, const Menu::arg *argv);      // in Log.pde
-static int8_t   setup_mode(uint8_t argc, const Menu::arg *argv);        // in setup.pde
-static int8_t   test_mode(uint8_t argc, const Menu::arg *argv);         // in test.cpp
-static int8_t   reboot_board(uint8_t argc, const Menu::arg *argv);
-
 // This is the help function
 // PSTR is an AVR macro to read strings from flash memory
 // printf_P is a version of print_f that reads from flash memory
-static int8_t   main_menu_help(uint8_t argc, const Menu::arg *argv)
+int8_t Plane::main_menu_help(uint8_t argc, const Menu::arg *argv)
 {
     cliSerial->printf_P(PSTR("Commands:\n"
                          "  logs        log readback/setup mode\n"
@@ -32,24 +29,24 @@ static int8_t   main_menu_help(uint8_t argc, const Menu::arg *argv)
 static const struct Menu::command main_menu_commands[] PROGMEM = {
 //   command		function called
 //   =======        ===============
-    {"logs",                process_logs},
+    {"logs",        MENU_FUNC(process_logs)},
-    {"setup",               setup_mode},
+    {"setup",       MENU_FUNC(setup_mode)},
-    {"test",                test_mode},
+    {"test",        MENU_FUNC(test_mode)},
-    {"reboot",              reboot_board},
+    {"reboot",      MENU_FUNC(reboot_board)},
-    {"help",                main_menu_help},
+    {"help",        MENU_FUNC(main_menu_help)},
 };
 
 // Create the top-level menu object.
 MENU(main_menu, THISFIRMWARE, main_menu_commands);
 
-static int8_t reboot_board(uint8_t argc, const Menu::arg *argv)
+int8_t Plane::reboot_board(uint8_t argc, const Menu::arg *argv)
 {
     hal.scheduler->reboot(false);
     return 0;
 }
 
 // the user wants the CLI. It never exits
-static void run_cli(AP_HAL::UARTDriver *port)
+void Plane::run_cli(AP_HAL::UARTDriver *port)
 {
     // disable the failsafe code in the CLI
     hal.scheduler->register_timer_failsafe(NULL,1);
@@ -68,7 +65,18 @@ static void run_cli(AP_HAL::UARTDriver *port)
 
 #endif // CLI_ENABLED
 
-static void init_ardupilot()
+
+static void mavlink_delay_cb_static()
+{
+    plane.mavlink_delay_cb();
+}
+
+static void failsafe_check_static()
+{
+    plane.failsafe_check();
+}
+
+void Plane::init_ardupilot()
 {
     // initialise serial port
     serial_manager.init_console();
@@ -148,18 +156,7 @@ static void init_ardupilot()
     mavlink_system.sysid = g.sysid_this_mav;
 
 #if LOGGING_ENABLED == ENABLED
-    DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
+    log_init();
-    if (!DataFlash.CardInserted()) {
-        gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash card inserted"));
-        g.log_bitmask.set(0);
-    } else if (DataFlash.NeedErase()) {
-        gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
-        do_erase_logs();
-        for (uint8_t i=0; i<num_gcs; i++) {
-            gcs[i].reset_cli_timeout();
-        }
-    }
-    arming.set_logging_available(DataFlash.CardInserted());
 #endif
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
@@ -183,7 +180,7 @@ static void init_ardupilot()
 
     // Register mavlink_delay_cb, which will run anytime you have
     // more than 5ms remaining in your call to hal.scheduler->delay
-    hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
+    hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
 
     // give AHRS the airspeed sensor
     ahrs.set_airspeed(&airspeed);
@@ -210,7 +207,7 @@ static void init_ardupilot()
      *  setup the 'main loop is dead' check. Note that this relies on
      *  the RC library being initialised.
      */
-    hal.scheduler->register_timer_failsafe(failsafe_check, 1000);
+    hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
 
 #if CLI_ENABLED == ENABLED
     if (g.cli_enabled == 1) {
@@ -248,7 +245,7 @@ static void init_ardupilot()
 //********************************************************************************
 //This function does all the calibrations, etc. that we need during a ground start
 //********************************************************************************
-static void startup_ground(void)
+void Plane::startup_ground(void)
 {
     set_mode(INITIALISING);
 
@@ -292,7 +289,7 @@ static void startup_ground(void)
 
     // reset last heartbeat time, so we don't trigger failsafe on slow
     // startup
-    failsafe.last_heartbeat_ms = millis();
+    failsafe.last_heartbeat_ms = hal.scheduler->millis();
 
     // we don't want writes to the serial port to cause us to pause
     // mid-flight, so set the serial ports non-blocking once we are
@@ -305,11 +302,11 @@ static void startup_ground(void)
     gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
 }
 
-static enum FlightMode get_previous_mode() {
+enum FlightMode Plane::get_previous_mode() {
     return previous_mode; 
 }
 
-static void set_mode(enum FlightMode mode)
+void Plane::set_mode(enum FlightMode mode)
 {
     if(control_mode == mode) {
         // don't switch modes if we are already in the correct mode.
@@ -440,7 +437,7 @@ static void set_mode(enum FlightMode mode)
 /*
   set_mode() wrapper for MAVLink SET_MODE
  */
-static bool mavlink_set_mode(uint8_t mode)
+bool Plane::mavlink_set_mode(uint8_t mode)
 {
     switch (mode) {
     case MANUAL:
@@ -463,7 +460,7 @@ static bool mavlink_set_mode(uint8_t mode)
 }
 
 // exit_mode - perform any cleanup required when leaving a flight mode
-static void exit_mode(enum FlightMode mode)
+void Plane::exit_mode(enum FlightMode mode)
 {
     // stop mission when we leave auto
     if (mode == AUTO) {
@@ -473,9 +470,9 @@ static void exit_mode(enum FlightMode mode)
     }
 }
 
-static void check_long_failsafe()
+void Plane::check_long_failsafe()
 {
-    uint32_t tnow = millis();
+    uint32_t tnow = hal.scheduler->millis();
     // only act on changes
     // -------------------
     if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS) {
@@ -503,7 +500,7 @@ static void check_long_failsafe()
     }
 }
 
-static void check_short_failsafe()
+void Plane::check_short_failsafe()
 {
     // only act on changes
     // -------------------
@@ -521,7 +518,7 @@ static void check_short_failsafe()
 }
 
 
-static void startup_INS_ground(void)
+void Plane::startup_INS_ground(void)
 {
     if (g.hil_mode == 1) {
         while (barometer.get_last_update() == 0) {
@@ -542,7 +539,7 @@ static void startup_INS_ground(void)
 
     if (style == AP_InertialSensor::COLD_START) {
         gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane"));
-        mavlink_delay(100);
+        hal.scheduler->delay(100);
     }
 
     ahrs.init();
@@ -568,21 +565,21 @@ static void startup_INS_ground(void)
 
 // updates the status of the notify objects
 // should be called at 50hz
-static void update_notify()
+void Plane::update_notify()
 {
     notify.update();
 }
 
-static void resetPerfData(void) 
+void Plane::resetPerfData(void) 
 {
     mainLoop_count                  = 0;
     G_Dt_max                        = 0;
     G_Dt_min                        = 0;
-    perf_mon_timer                  = millis();
+    perf_mon_timer                  = hal.scheduler->millis();
 }
 
 
-static void check_usb_mux(void)
+void Plane::check_usb_mux(void)
 {
     bool usb_check = hal.gpio->usb_connected();
     if (usb_check == usb_connected) {
@@ -606,8 +603,7 @@ static void check_usb_mux(void)
 }
 
 
-static void
+void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
-print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
 {
     switch (mode) {
     case MANUAL:
@@ -656,7 +652,7 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
 }
 
 #if CLI_ENABLED == ENABLED
-static void print_comma(void)
+void Plane::print_comma(void)
 {
     cliSerial->print_P(PSTR(","));
 }
@@ -665,7 +661,7 @@ static void print_comma(void)
 /*
   write to a servo
  */
-static void servo_write(uint8_t ch, uint16_t pwm)
+void Plane::servo_write(uint8_t ch, uint16_t pwm)
 {
     if (g.hil_mode==1 && !g.hil_servos) {
         if (ch < 8) {
@@ -680,7 +676,7 @@ static void servo_write(uint8_t ch, uint16_t pwm)
 /*
   should we log a message type now?
  */
-static bool should_log(uint32_t mask)
+bool Plane::should_log(uint32_t mask)
 {
     if (!(mask & g.log_bitmask) || in_mavlink_delay) {
         return false;
@@ -702,7 +698,7 @@ static bool should_log(uint32_t mask)
   send FrSky telemetry. Should be called at 5Hz by scheduler
  */
 #if FRSKY_TELEM_ENABLED == ENABLED
-static void frsky_telemetry_send(void)
+void Plane::frsky_telemetry_send(void)
 {
     frsky_telemetry.send_frames((uint8_t)control_mode);
 }
@@ -712,7 +708,7 @@ static void frsky_telemetry_send(void)
 /*
   return throttle percentage from 0 to 100
  */
-static uint8_t throttle_percentage(void)
+uint8_t Plane::throttle_percentage(void)
 {
     // to get the real throttle we need to use norm_output() which
     // returns a number from -1 to 1.
@@ -722,7 +718,7 @@ static uint8_t throttle_percentage(void)
 /*
   update AHRS soft arm state and log as needed
  */
-static void change_arm_state(void)
+void Plane::change_arm_state(void)
 {
     Log_Arm_Disarm();
     hal.util->set_soft_armed(arming.is_armed() &&
@@ -737,7 +733,7 @@ static void change_arm_state(void)
 /*
   arm motors
  */
-static bool arm_motors(AP_Arming::ArmingMethod method)
+bool Plane::arm_motors(AP_Arming::ArmingMethod method)
 {
     if (!arming.arm(method)) {
         return false;
@@ -752,7 +748,7 @@ static bool arm_motors(AP_Arming::ArmingMethod method)
 /*
   disarm motors
  */
-static bool disarm_motors(void)
+bool Plane::disarm_motors(void)
 {
     if (!arming.disarm()) {
         return false;

ArduPlane/takeoff.cpp

@@ -1,5 +1,7 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Plane.h"
+
 /*   Check for automatic takeoff conditions being met using the following sequence:
  *   1) Check for adequate GPS lock - if not return false
  *   2) Check the gravity compensated longitudinal acceleration against the threshold and start the timer if true
@@ -9,7 +11,7 @@
  *   6) If the time lapsed since the last timecheck is greater than 0.2 seconds, return false and reset the timer
  *   NOTE : This function relies on the TECS 50Hz processing for its acceleration measure.
  */
-static bool auto_takeoff_check(void)
+bool Plane::auto_takeoff_check(void)
 {
     // this is a more advanced check that relies on TECS
     uint32_t now = hal.scheduler->millis();
@@ -85,7 +87,7 @@ no_launch:
 /*
   calculate desired bank angle during takeoff, setting nav_roll_cd
  */
-static void takeoff_calc_roll(void)
+void Plane::takeoff_calc_roll(void)
 {
     if (steer_state.hold_course_cd == -1) {
         // we don't yet have a heading to hold - just level
@@ -105,7 +107,7 @@ static void takeoff_calc_roll(void)
 /*
   calculate desired pitch angle during takeoff, setting nav_pitch_cd
  */
-static void takeoff_calc_pitch(void)
+void Plane::takeoff_calc_pitch(void)
 {
     if (auto_state.highest_airspeed < g.takeoff_rotate_speed) {
         // we have not reached rotate speed, use a target pitch of 5
@@ -134,7 +136,7 @@ static void takeoff_calc_pitch(void)
   This can be used either in auto-takeoff or in FBWA mode with
   FBWA_TDRAG_CHAN enabled
  */
-static int8_t takeoff_tail_hold(void)
+int8_t Plane::takeoff_tail_hold(void)
 {
     bool in_takeoff = ((control_mode == AUTO && !auto_state.takeoff_complete) ||
                        (control_mode == FLY_BY_WIRE_A && auto_state.fbwa_tdrag_takeoff_mode));

ArduPlane/test.cpp

@@ -1,60 +1,36 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include "Plane.h"
+
 #if CLI_ENABLED == ENABLED
 
-// These are function definitions so the Menu can be constructed before the functions
-// are defined below. Order matters to the compiler.
-static int8_t   test_radio_pwm(uint8_t argc,    const Menu::arg *argv);
-static int8_t   test_radio(uint8_t argc,                const Menu::arg *argv);
-static int8_t   test_passthru(uint8_t argc,     const Menu::arg *argv);
-static int8_t   test_failsafe(uint8_t argc,     const Menu::arg *argv);
-static int8_t   test_gps(uint8_t argc,                  const Menu::arg *argv);
-#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
-static int8_t   test_adc(uint8_t argc,                  const Menu::arg *argv);
-#endif
-static int8_t   test_ins(uint8_t argc,                  const Menu::arg *argv);
-static int8_t   test_relay(uint8_t argc,                const Menu::arg *argv);
-static int8_t   test_wp(uint8_t argc,                   const Menu::arg *argv);
-static int8_t   test_airspeed(uint8_t argc,     const Menu::arg *argv);
-static int8_t   test_pressure(uint8_t argc,     const Menu::arg *argv);
-static int8_t   test_mag(uint8_t argc,                  const Menu::arg *argv);
-static int8_t   test_xbee(uint8_t argc,                 const Menu::arg *argv);
-static int8_t   test_modeswitch(uint8_t argc,           const Menu::arg *argv);
-static int8_t   test_logging(uint8_t argc,              const Menu::arg *argv);
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
-static int8_t   test_shell(uint8_t argc,              const Menu::arg *argv);
-#endif
-
-// forward declaration to keep the compiler happy
-static void test_wp_print(const AP_Mission::Mission_Command& cmd);
-
 // Creates a constant array of structs representing menu options
 // and stores them in Flash memory, not RAM.
 // User enters the string in the console to call the functions on the right.
 // See class Menu in AP_Common for implementation details
 static const struct Menu::command test_menu_commands[] PROGMEM = {
-    {"pwm",                         test_radio_pwm},
+    {"pwm",                 MENU_FUNC(test_radio_pwm)},
-    {"radio",                       test_radio},
+    {"radio",               MENU_FUNC(test_radio)},
-    {"passthru",            test_passthru},
+    {"passthru",            MENU_FUNC(test_passthru)},
-    {"failsafe",            test_failsafe},
+    {"failsafe",            MENU_FUNC(test_failsafe)},
-    {"relay",                       test_relay},
+    {"relay",               MENU_FUNC(test_relay)},
-    {"waypoints",           test_wp},
+    {"waypoints",           MENU_FUNC(test_wp)},
-    {"xbee",                        test_xbee},
+    {"xbee",                MENU_FUNC(test_xbee)},
-    {"modeswitch",          test_modeswitch},
+    {"modeswitch",          MENU_FUNC(test_modeswitch)},
 
     // Tests below here are for hardware sensors only present
     // when real sensors are attached or they are emulated
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
-    {"adc",                 test_adc},
+    {"adc",                 MENU_FUNC(test_adc)},
 #endif
-    {"gps",                 test_gps},
+    {"gps",                 MENU_FUNC(test_gps)},
-    {"ins",                 test_ins},
+    {"ins",                 MENU_FUNC(test_ins)},
-    {"airspeed",    test_airspeed},
+    {"airspeed",            MENU_FUNC(test_airspeed)},
-    {"airpressure", test_pressure},
+    {"airpressure",         MENU_FUNC(test_pressure)},
-    {"compass",             test_mag},
+    {"compass",             MENU_FUNC(test_mag)},
-    {"logging",             test_logging},
+    {"logging",             MENU_FUNC(test_logging)},
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
-    {"shell", 				test_shell},
+    {"shell", 				MENU_FUNC(test_shell)},
 #endif
 
 };
@@ -62,21 +38,19 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
 // A Macro to create the Menu
 MENU(test_menu, "test", test_menu_commands);
 
-static int8_t
+int8_t Plane::test_mode(uint8_t argc, const Menu::arg *argv)
-test_mode(uint8_t argc, const Menu::arg *argv)
 {
     cliSerial->printf_P(PSTR("Test Mode\n\n"));
     test_menu.run();
     return 0;
 }
 
-static void print_hit_enter()
+void Plane::print_hit_enter()
 {
     cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
 }
 
-static int8_t
+int8_t Plane::test_radio_pwm(uint8_t argc, const Menu::arg *argv)
-test_radio_pwm(uint8_t argc, const Menu::arg *argv)
 {
     print_hit_enter();
     hal.scheduler->delay(1000);
@@ -105,8 +79,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
 }
 
 
-static int8_t
+int8_t Plane::test_passthru(uint8_t argc, const Menu::arg *argv)
-test_passthru(uint8_t argc, const Menu::arg *argv)
 {
     print_hit_enter();
     hal.scheduler->delay(1000);
@@ -131,8 +104,7 @@ test_passthru(uint8_t argc, const Menu::arg *argv)
     return 0;
 }
 
-static int8_t
+int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv)
-test_radio(uint8_t argc, const Menu::arg *argv)
 {
     print_hit_enter();
     hal.scheduler->delay(1000);
@@ -170,8 +142,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
     }
 }
 
-static int8_t
+int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
-test_failsafe(uint8_t argc, const Menu::arg *argv)
 {
     uint8_t fail_test;
     print_hit_enter();
@@ -225,8 +196,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
     }
 }
 
-static int8_t
+int8_t Plane::test_relay(uint8_t argc, const Menu::arg *argv)
-test_relay(uint8_t argc, const Menu::arg *argv)
 {
     print_hit_enter();
     hal.scheduler->delay(1000);
@@ -248,8 +218,7 @@ test_relay(uint8_t argc, const Menu::arg *argv)
     }
 }
 
-static int8_t
+int8_t Plane::test_wp(uint8_t argc, const Menu::arg *argv)
-test_wp(uint8_t argc, const Menu::arg *argv)
 {
     hal.scheduler->delay(1000);
 
@@ -274,8 +243,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
     return (0);
 }
 
-static void
+void Plane::test_wp_print(const AP_Mission::Mission_Command& cmd)
-test_wp_print(const AP_Mission::Mission_Command& cmd)
 {
     cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
                     (int)cmd.index,
@@ -287,8 +255,7 @@ test_wp_print(const AP_Mission::Mission_Command& cmd)
                     (long)cmd.content.location.lng);
 }
 
-static int8_t
+int8_t Plane::test_xbee(uint8_t argc, const Menu::arg *argv)
-test_xbee(uint8_t argc, const Menu::arg *argv)
 {
     print_hit_enter();
     hal.scheduler->delay(1000);
@@ -306,8 +273,7 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
 }
 
 
-static int8_t
+int8_t Plane::test_modeswitch(uint8_t argc, const Menu::arg *argv)
-test_modeswitch(uint8_t argc, const Menu::arg *argv)
 {
     print_hit_enter();
     hal.scheduler->delay(1000);
@@ -332,8 +298,7 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
 /*
  *  test the dataflash is working
  */
-static int8_t
+int8_t Plane::test_logging(uint8_t argc, const Menu::arg *argv)
-test_logging(uint8_t argc, const Menu::arg *argv)
 {
     DataFlash.ShowDeviceInfo(cliSerial);
     return 0;
@@ -343,8 +308,7 @@ test_logging(uint8_t argc, const Menu::arg *argv)
 /*
  *  run a debug shell
  */
-static int8_t
+int8_t Plane::test_shell(uint8_t argc, const Menu::arg *argv)
-test_shell(uint8_t argc, const Menu::arg *argv)
 {
     hal.util->run_debug_shell(cliSerial);
     return 0;
@@ -355,8 +319,7 @@ test_shell(uint8_t argc, const Menu::arg *argv)
 // tests in this section are for real sensors or sensors that have been simulated
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
-static int8_t
+int8_t Plane::test_adc(uint8_t argc, const Menu::arg *argv)
-test_adc(uint8_t argc, const Menu::arg *argv)
 {
     print_hit_enter();
     apm1_adc.Init();
@@ -375,8 +338,7 @@ test_adc(uint8_t argc, const Menu::arg *argv)
 }
 #endif
 
-static int8_t
+int8_t Plane::test_gps(uint8_t argc, const Menu::arg *argv)
-test_gps(uint8_t argc, const Menu::arg *argv)
 {
     print_hit_enter();
     hal.scheduler->delay(1000);
@@ -404,8 +366,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
     }
 }
 
-static int8_t
+int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv)
-test_ins(uint8_t argc, const Menu::arg *argv)
 {
     //cliSerial->printf_P(PSTR("Calibrating."));
     ahrs.init();
@@ -456,8 +417,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
 }
 
 
-static int8_t
+int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
-test_mag(uint8_t argc, const Menu::arg *argv)
 {
     if (!g.compass_enabled) {
         cliSerial->printf_P(PSTR("Compass: "));
@@ -473,7 +433,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
     ahrs.set_fly_forward(true);
     ahrs.set_wind_estimation(true);
     ahrs.set_compass(&compass);
-    report_compass();
 
     // we need the AHRS initialised for this test
     ins.init(AP_InertialSensor::COLD_START, 
@@ -533,8 +492,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 //-------------------------------------------------------------------------------------------
 // real sensors that have not been simulated yet go here
 
-static int8_t
+int8_t Plane::test_airspeed(uint8_t argc, const Menu::arg *argv)
-test_airspeed(uint8_t argc, const Menu::arg *argv)
 {
     if (!airspeed.enabled()) {
         cliSerial->printf_P(PSTR("airspeed: "));
@@ -559,8 +517,7 @@ test_airspeed(uint8_t argc, const Menu::arg *argv)
 }
 
 
-static int8_t
+int8_t Plane::test_pressure(uint8_t argc, const Menu::arg *argv)
-test_pressure(uint8_t argc, const Menu::arg *argv)
 {
     cliSerial->printf_P(PSTR("Uncalibrated relative airpressure\n"));
     print_hit_enter();
@@ -586,4 +543,14 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
     }
 }
 
+void Plane::print_enabled(bool b)
+{
+    if (b) {
+        cliSerial->printf_P(PSTR("en"));
+    } else {
+        cliSerial->printf_P(PSTR("dis"));
+    }
+    cliSerial->printf_P(PSTR("abled\n"));
+}
+
 #endif // CLI_ENABLED