diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp
new file mode 100644
index 0000000000..c34a0aca4d
--- /dev/null
+++ b/APMrover2/Rover.cpp
@@ -0,0 +1,90 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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 .
+*/
+/*
+ main Rover class, containing all vehicle specific state
+*/
+
+
+/*
+ scheduler table - all regular tasks should be 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 Rover::scheduler_tasks[] PROGMEM = {
+ { read_radio, 1, 1000 },
+ { ahrs_update, 1, 6400 },
+ { read_sonars, 1, 2000 },
+ { update_current_mode, 1, 1500 },
+ { set_servos, 1, 1500 },
+ { update_GPS_50Hz, 1, 2500 },
+ { update_GPS_10Hz, 5, 2500 },
+ { update_alt, 5, 3400 },
+ { navigate, 5, 1600 },
+ { update_compass, 5, 2000 },
+ { update_commands, 5, 1000 },
+ { update_logging1, 5, 1000 },
+ { update_logging2, 5, 1000 },
+ { gcs_retry_deferred, 1, 1000 },
+ { gcs_update, 1, 1700 },
+ { gcs_data_stream_send, 1, 3000 },
+ { read_control_switch, 15, 1000 },
+ { read_trim_switch, 5, 1000 },
+ { read_battery, 5, 1000 },
+ { read_receiver_rssi, 5, 1000 },
+ { update_events, 1, 1000 },
+ { check_usb_mux, 15, 1000 },
+ { mount_update, 1, 600 },
+ { gcs_failsafe_check, 5, 600 },
+ { compass_accumulate, 1, 900 },
+ { update_notify, 1, 300 },
+ { one_second_loop, 50, 3000 },
+#if FRSKY_TELEM_ENABLED == ENABLED
+ { frsky_telemetry_send, 10, 100 }
+#endif
+};
+
+
+Rover::Rover(void) :
+ param_loader(var_info)
+ channel_steer(NULL),
+ channel_throttle(NULL),
+ channel_learn(NULL),
+ in_log_download(false),
+ modes(&g.mode1),
+#if AP_AHRS_NAVEKF_AVAILABLE
+ ahrs(ins, barometer, gps, sonar),
+#else
+ ahrs(ins, barometer, gps),
+#endif
+ L1_controller(ahrs),
+ nav_controller(&L1_controller),
+ steerController(ahrs),
+ mission(ahrs, &start_command, &verify_command, &exit_mission),
+ ServoRelayEvents(relay),
+#if CAMERA == ENABLED
+ AP_camera(&relay),
+#endif
+#if MOUNT == ENABLED
+ camera_mount(ahrs, current_loc),
+#endif
+ control_mode(INITIALISING),
+ ground_start_count(20),
+ throttle(500),
+ frsky_telemetry(ahrs, battery),
+ home(ahrs.get_home()),
+ G_Dt(0.02),
+{
+}
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
new file mode 100644
index 0000000000..92774aecc6
--- /dev/null
+++ b/APMrover2/Rover.h
@@ -0,0 +1,419 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ 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 .
+*/
+/*
+ main Rover class, containing all vehicle specific state
+*/
+
+class Rover {
+public:
+ Rover(void);
+ // public member functions
+ void setup(void);
+ void loop(void);
+
+private:
+ AP_HAL::BetterStream* cliSerial;
+
+ // must be the first AP_Param variable declared to ensure its
+ // constructor runs before the constructors of the other AP_Param
+ // variables
+ AP_Param param_loader;
+
+ // the rate we run the main loop at
+ const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ;
+
+ // all settable parameters
+ 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_steer;
+ RC_Channel *channel_throttle;
+ RC_Channel *channel_learn;
+
+ // 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(HAL_BOARD_LOG_DIRECTORY);
+#else
+ DataFlash_Empty DataFlash;
+#endif
+
+ bool in_log_download;
+
+ // sensor drivers
+ AP_GPS gps;
+ AP_Baro barometer;
+ Compass compass;
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+ AP_ADC_ADS7844 apm1_adc;
+#endif
+ AP_InertialSensor ins;
+ RangeFinder sonar;
+
+ // flight modes convenience array
+ AP_Int8 *modes;
+
+ // Inertial Navigation EKF
+#if AP_AHRS_NAVEKF_AVAILABLE
+ AP_AHRS_NavEKF ahrs;
+#else
+ AP_AHRS_DCM ahrs;
+#endif
+
+ AP_L1_Control L1_controller;
+
+ // selected navigation controller
+ AP_Navigation *nav_controller;
+
+ // steering controller
+ AP_SteerController steerController;
+
+ // Mission library
+ AP_Mission mission;
+
+ OpticalFlow optflow;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+ SITL sitl;
+#endif
+
+ // GCS handling
+ AP_SerialManager serial_manager;
+ const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
+ GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
+
+ // a pin for reading the receiver RSSI voltage. The scaling by 0.25
+ // is to take the 0 to 1024 range down to an 8 bit range for MAVLink
+ AP_HAL::AnalogSource *rssi_analog_source;
+
+ // relay support
+ AP_Relay relay;
+
+ AP_ServoRelayEvents ServoRelayEvents;
+
+ // Camera
+#if CAMERA == ENABLED
+ AP_Camera camera;
+#endif
+
+ // The rover's current location
+ struct Location current_loc;
+
+ // 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
+
+ // if USB is connected
+ bool usb_connected;
+
+ // Radio
+ // This is the state of the flight control system
+ // There are multiple states defined such as MANUAL, FBW-A, AUTO
+ enum mode control_mode;
+
+ // Used to maintain the state of the previous control switch position
+ // This is set to -1 when we need to re-read the switch
+ uint8_t oldSwitchPosition;
+
+ // These are values received from the GCS if the user is using GCS joystick
+ // control and are substituted for the values coming from the RC radio
+ int16_t rc_override[8];
+
+ // A flag if GCS joystick control is in use
+ bool rc_override_active;
+
+ // Failsafe
+ // A tracking variable for type of failsafe active
+ // Used for failsafe based on loss of RC signal or GCS signal. See
+ // FAILSAFE_EVENT_*
+ static struct {
+ uint8_t bits;
+ uint32_t rc_override_timer;
+ uint32_t start_time;
+ uint8_t triggered;
+ uint32_t last_valid_rc_ms;
+ } failsafe;
+
+ // notification object for LEDs, buzzers etc (parameter set to false disables external leds)
+ AP_Notify notify;
+
+ // 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;
+
+ // Location & Navigation
+ const float radius_of_earth = 6378100; // meters
+
+ // true if we have a position estimate from AHRS
+ bool have_position;
+
+ bool rtl_complete;
+
+ // angle of our next navigation waypoint
+ int32_t next_navigation_leg_cd;
+
+ // ground speed error in m/s
+ float groundspeed_error;
+
+ // 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;
+
+ // the time when the last HEARTBEAT message arrived from a GCS
+ uint32_t last_heartbeat_ms;
+
+ // obstacle detection information
+ struct {
+ // have we detected an obstacle?
+ uint8_t detected_count;
+ float turn_angle;
+ uint16_t sonar1_distance_cm;
+ uint16_t sonar2_distance_cm;
+
+ // time when we last detected an obstacle, in milliseconds
+ uint32_t detected_time_ms;
+ } obstacle;
+
+ // this is set to true when auto has been triggered to start
+ bool auto_triggered;
+
+ // Ground speed
+ // The amount current ground speed is below min ground speed. meters per second
+ float ground_speed;
+ int16_t throttle_last;
+ int16_t throttle;
+
+ // CH7 control
+ // Used to track the CH7 toggle state.
+ // When CH7 goes LOW PWM from HIGH PWM, this value will have been set true
+ // This allows advanced functionality to know when to execute
+ bool ch7_flag;
+
+ // Battery Sensors
+ AP_BattMonitor battery;
+
+ // Battery Sensors
+#if FRSKY_TELEM_ENABLED == ENABLED
+ AP_Frsky_Telem frsky_telemetry;
+#endif
+
+ // Navigation control variables
+ // The instantaneous desired lateral acceleration in m/s/s
+ float lateral_acceleration;
+
+ // Waypoint distances
+ // Distance between rover and next waypoint. Meters
+ float wp_distance;
+ // Distance between previous and next waypoint. Meters
+ int32_t wp_totalDistance;
+
+ // 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;
+ // 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
+ int32_t condition_start;
+
+ // 3D Location vectors
+ // Location structure defined in AP_Common
+ // The home location used for RTL. The location is set when we first get stable GPS lock
+ const struct Location &home;
+
+ // Flag for if we have gps lock and have set the home location
+ bool home_is_set;
+ // The location of the previous waypoint. Used for track following and altitude ramp calculations
+ struct Location prev_WP;
+ // The location of the current/active waypoint. Used for track following
+ struct Location next_WP;
+ // The location of the active waypoint in Guided mode.
+ struct Location guided_WP;
+
+ // IMU 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
+ int32_t perf_mon_timer;
+ // The maximum main loop execution time recorded in the current performance monitoring interval
+ uint32_t G_Dt_max;
+
+ // 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;
+
+ // set if we are driving backwards
+ bool in_reverse;
+
+ static const AP_Scheduler::Task scheduler_tasks[];
+
+private:
+ // private member functions
+ void ahrs_update();
+ void mount_update(void);
+ void update_alt();
+ void gcs_failsafe_check(void);
+ void compass_accumulate(void);
+ void update_compass(void);
+ void update_logging1(void);
+ void update_logging2(void);
+ void update_aux(void);
+ void one_second_loop(void);
+ void update_GPS_50Hz(void);
+ void update_GPS_10Hz(void);
+ void update_current_mode(void);
+ void update_navigation();
+ void send_heartbeat(mavlink_channel_t chan);
+ void send_attitude(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_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 mavlink_delay_cb();
+ 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_retry_deferred(void);
+ bool print_log_menu(void);
+ void do_erase_logs(void);
+ void Log_Write_Performance();
+ void Log_Write_Steering();
+ void Log_Write_Startup(uint8_t type);
+ void Log_Write_EntireMission();
+ void Log_Write_Control_Tuning();
+ void Log_Write_Nav_Tuning();
+ void Log_Write_Attitude();
+ void Log_Write_Sonar();
+ void Log_Write_Current();
+ void Log_Write_RC(void);
+ void Log_Write_Baro(void);
+ void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
+ void start_logging() ;
+ void Log_Write_Startup(uint8_t type);
+ void Log_Write_EntireMission();
+ void Log_Write_Current();
+ void Log_Write_Nav_Tuning();
+ void Log_Write_Performance();
+ void Log_Write_Control_Tuning();
+ void Log_Write_Sonar();
+ void Log_Write_Attitude();
+ void start_logging();
+ void Log_Write_RC(void);
+ void load_parameters(void);
+ void throttle_slew_limit(int16_t last_throttle);
+ bool auto_check_trigger(void);
+ bool use_pivot_steering(void);
+ void calc_throttle(float target_speed);
+ void calc_lateral_acceleration();
+ void calc_nav_steer();
+ void set_servos(void);
+ void set_next_WP(const struct Location& loc);
+ void set_guided_WP(void);
+ void init_home();
+ void restart_nav();
+ void exit_mission();
+ void do_RTL(void);
+ bool verify_RTL();
+ bool verify_wait_delay();
+ bool verify_within_distance();
+ void do_take_picture();
+ void log_picture();
+ void update_commands(void);
+ void delay(uint32_t ms);
+ void mavlink_delay(uint32_t ms);
+ uint32_t millis();
+ uint32_t micros();
+ void read_control_switch();
+ uint8_t readSwitch(void);
+ void reset_control_switch();
+ void read_trim_switch();
+ void update_events(void);
+ void failsafe_check();
+ void navigate();
+ void reached_waypoint();
+ void set_control_channels(void);
+ void init_rc_in();
+ void init_rc_out();
+ void read_radio();
+ void control_failsafe(uint16_t pwm);
+ void trim_control_surfaces();
+ void trim_radio();
+ void init_barometer(void);
+ void init_sonar(void);
+ void read_battery(void);
+ void read_receiver_rssi(void);
+ void read_sonars(void);
+ void report_batt_monitor();
+ void report_radio();
+ void report_gains();
+ void report_throttle();
+ void report_compass();
+ void report_modes();
+ void print_PID(PID * pid);
+ void print_radio_values();
+ void print_switch(uint8_t p, uint8_t m);
+ void print_done();
+ void print_blanks(int num);
+ void print_divider(void);
+ int8_t radio_input_switch(void);
+ void zero_eeprom(void);
+ void print_enabled(bool b);
+ void init_ardupilot();
+ void startup_ground(void);
+ void set_reverse(bool reverse);
+ void set_mode(enum mode mode);
+ bool mavlink_set_mode(uint8_t mode);
+ void failsafe_trigger(uint8_t failsafe_type, bool on);
+ void startup_INS_ground(void);
+ void update_notify();
+ void resetPerfData(void);
+ void check_usb_mux(void);
+ uint8_t check_digital_pin(uint8_t pin);
+ bool should_log(uint32_t mask);
+ void frsky_telemetry_send(void);
+ void print_hit_enter();
+};