.editorconfig

+# This file provide reference settings for the APM code conventions
+# There are plug-ins available for nearly every text editor to automatically
+# respect the conventions contained within this file.
+#
+# Please see editorconfig.org for complete information.
+#
+# If you find errors in this file, please send a pull-request with a fix.
+#
+root = true
+
+[*]
+indent_style = space
+indent_size = 4
+end_of_line = lf
+charset = utf-8
+trim_trailing_whitespace = false # These are the correct rules for APM coding standards, but fixing up old files causes git spam
+insert_final_newline = false
+
+[*.mk]
+indent_style = tab
+indent_size = 8
+
+[{makefile,Makefile}]
+indent_style = tab
+indent_size = 8

.gitignore

+*~
+*.bin
+*.d
+*.dll
+*.elf
+*.hex
+*.dfu
+*.exe
+*.lst
+*.o
+*.obj
+*.px4
+*.vrx
+*.pyc
+*.tlog
+*.tlog.raw
+*.vbrain
+*.zip
+.*.swo
+.*.swp
+.built
+.context
+.cproject
+.depend
+.directory
+.DS_Store
+.metadata/
+.project
+.settings/
+.vagrant
+/tmp/*
+/Tools/ArdupilotMegaPlanner/3DRRadio/bin
+/Tools/ArdupilotMegaPlanner/3DRRadio/obj
+/Tools/ArdupilotMegaPlanner/APMPlannerXplanes/APMPlannerXplanes/Debug
+/Tools/ArdupilotMegaPlanner/bin
+/Tools/ArdupilotMegaPlanner/bin/APM Planner.app
+/Tools/ArdupilotMegaPlanner/bin/Debug
+/Tools/ArdupilotMegaPlanner/bin/Release/gmapcache
+/Tools/ArdupilotMegaPlanner/bin/Release/logs/
+/Tools/ArdupilotMegaPlanner/CustomImages
+/Tools/ArdupilotMegaPlanner/Msi/upload.bat
+/Tools/ArdupilotMegaPlanner/obj
+/Tools/ArdupilotMegaPlanner/resedit/bin
+/Tools/ArdupilotMegaPlanner/resedit/obj
+/Tools/ArdupilotMegaPlanner/Updater/bin
+/Tools/ArdupilotMegaPlanner/Updater/obj
+/Tools/ArdupilotMegaPlanner/wix/bin
+/Tools/ArdupilotMegaPlanner/wix/obj
+/Tools/autotest/ch7_mission.txt
+/Tools/autotest/aircraft/Rascal/reset.xml
+/Tools/autotest/jsbsim/fgout.xml
+/Tools/autotest/jsbsim/rascal_test.xml
+ArduCopter/Debug/
+ArduCopter/terrain/*.DAT
+ArduCopter/test.ArduCopter/
+ArduCopter/test/*
+ArduPlane/terrain/*.DAT
+ArduPlane/test.ArduPlane/
+APMrover2/test.APMrover2/
+autotest.lck
+build
+Build.ArduCopter/*
+Build.ArduPlane/*
+Build.APMrover2/*
+Build.AntennaTracker/*
+cmake_install.cmake
+CMakeCache.txt
+CMakeFiles
+config.mk
+dataflash.bin
+eeprom.bin
+index.html
+LASTLOG.TXT
+Make.dep
+mav.log
+mav.log.raw
+module.mk
+serialsent.raw
+status.txt
+tags
+test.ArduCopter/*
+Thumbs.db

.pydevproject

+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?eclipse-pydev version="1.0"?><pydev_project>
+<pydev_property name="org.python.pydev.PYTHON_PROJECT_INTERPRETER">Default</pydev_property>
+<pydev_property name="org.python.pydev.PYTHON_PROJECT_VERSION">python 2.7</pydev_property>
+</pydev_project>

.travis.yml

+language: cpp
+
+before_install:
+  -  APMDIR=$(pwd) && pushd .. && $APMDIR/Tools/scripts/install-prereqs-ubuntu.sh -y && . ~/.profile && popd
+
+script: 
+  - Tools/scripts/build_all_travis.sh
+
+notifications:
+  webhooks:
+    urls:
+      - https://webhooks.gitter.im/e/e5e0b55e353e53945b4b
+    on_success: change  # options: [always|never|change] default: always
+    on_failure: always  # options: [always|never|change] default: always
+    on_start: false     # default: false

APMrover2/APM_Config.h

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// This file is just a placeholder for your configuration file.  If you wish to change any of the setup parameters from
+// their default values, place the appropriate #define statements here.
+
+

APMrover2/Log.pde

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#if LOGGING_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},
+	{"erase",	erase_logs},
+	{"enable",	select_logs},
+	{"disable",	select_logs}
+};
+
+// A Macro to create the Menu
+MENU2(log_menu, "Log", log_menu_commands, print_log_menu);
+
+static bool
+print_log_menu(void)
+{
+	cliSerial->printf_P(PSTR("logs enabled: "));
+
+	if (0 == g.log_bitmask) {
+		cliSerial->printf_P(PSTR("none"));
+	}else{
+		// Macro to make the following code a bit easier on the eye.
+		// Pass it the capitalised name of the log option, as defined
+		// in defines.h but without the LOG_ prefix.  It will check for
+		// the bit being set and print the name of the log option to suit.
+		#define PLOG(_s)	if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf_P(PSTR(" %S"), PSTR(#_s))
+		PLOG(ATTITUDE_FAST);
+		PLOG(ATTITUDE_MED);
+		PLOG(GPS);
+		PLOG(PM);
+		PLOG(CTUN);
+		PLOG(NTUN);
+		PLOG(MODE);
+		PLOG(IMU);
+		PLOG(CMD);
+		PLOG(CURRENT);
+		PLOG(SONAR);
+		PLOG(COMPASS);
+		PLOG(CAMERA);
+		PLOG(STEERING);
+		#undef PLOG
+	}
+
+	cliSerial->println();
+
+    DataFlash.ListAvailableLogs(cliSerial);
+	return(true);
+}
+
+static int8_t
+dump_log(uint8_t argc, const Menu::arg *argv)
+{
+    int16_t dump_log;
+    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;
+    last_log_num = DataFlash.find_last_log();
+
+    if (dump_log == -2) {
+        DataFlash.DumpPageInfo(cliSerial);
+        return(-1);
+    } else if (dump_log <= 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))
+    {
+        cliSerial->printf_P(PSTR("bad log number\n"));
+        return(-1);
+    }
+
+    DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
+    Log_Read((uint16_t)dump_log, dump_log_start, dump_log_end);
+    return 0;
+}
+
+
+static void do_erase_logs(void)
+{
+	cliSerial->printf_P(PSTR("\nErasing log...\n"));
+    DataFlash.EraseAll();
+	cliSerial->printf_P(PSTR("\nLog erased.\n"));
+}
+
+static int8_t
+erase_logs(uint8_t argc, const Menu::arg *argv)
+{
+    in_mavlink_delay = true;
+    do_erase_logs();
+    in_mavlink_delay = false;
+    return 0;
+}
+
+static int8_t
+select_logs(uint8_t argc, const Menu::arg *argv)
+{
+	uint16_t	bits;
+
+	if (argc != 2) {
+		cliSerial->printf_P(PSTR("missing log type\n"));
+		return(-1);
+	}
+
+	bits = 0;
+
+	// Macro to make the following code a bit easier on the eye.
+	// Pass it the capitalised name of the log option, as defined
+	// in defines.h but without the LOG_ prefix.  It will check for
+	// that name as the argument to the command, and set the bit in
+	// bits accordingly.
+	//
+	if (!strcasecmp_P(argv[1].str, PSTR("all"))) {
+		bits = ~0;
+	} else {
+		#define TARG(_s)	if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s
+		TARG(ATTITUDE_FAST);
+		TARG(ATTITUDE_MED);
+		TARG(GPS);
+		TARG(PM);
+		TARG(CTUN);
+		TARG(NTUN);
+		TARG(MODE);
+		TARG(IMU);
+		TARG(CMD);
+		TARG(CURRENT);
+		TARG(SONAR);
+		TARG(COMPASS);
+		TARG(CAMERA);
+		TARG(STEERING);
+		#undef TARG
+	}
+
+	if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
+		g.log_bitmask.set_and_save(g.log_bitmask | bits);
+	}else{
+		g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
+	}
+	return(0);
+}
+
+static int8_t
+process_logs(uint8_t argc, const Menu::arg *argv)
+{
+	log_menu.run();
+	return 0;
+}
+
+struct PACKED log_Performance {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint32_t loop_time;
+    uint16_t main_loop_count;
+    uint32_t g_dt_max;
+    int16_t  gyro_drift_x;
+    int16_t  gyro_drift_y;
+    int16_t  gyro_drift_z;
+    uint8_t  i2c_lockup_count;
+    uint16_t ins_error_count;
+};
+
+// Write a performance monitoring packet. Total length : 19 bytes
+static void Log_Write_Performance()
+{
+    struct log_Performance pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
+        time_ms         : millis(),
+        loop_time       : 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),
+        gyro_drift_y    : (int16_t)(ahrs.get_gyro_drift().y * 1000),
+        gyro_drift_z    : (int16_t)(ahrs.get_gyro_drift().z * 1000),
+        i2c_lockup_count: hal.i2c->lockup_count(),
+        ins_error_count  : ins.error_count()
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+// Write a mission command. Total length : 36 bytes
+static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd)
+{
+    mavlink_mission_item_t mav_cmd = {};
+    AP_Mission::mission_cmd_to_mavlink(cmd,mav_cmd);
+    DataFlash.Log_Write_MavCmd(mission.num_commands(),mav_cmd);
+}
+
+struct PACKED log_Steering {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    float demanded_accel;
+    float achieved_accel;
+};
+
+// Write a steering packet
+static void Log_Write_Steering()
+{
+    struct log_Steering pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_STEERING_MSG),
+        time_ms        : hal.scheduler->millis(),
+        demanded_accel : lateral_acceleration,
+        achieved_accel : gps.ground_speed() * ins.get_gyro().z,
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+struct PACKED log_Startup {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint8_t startup_type;
+    uint16_t command_total;
+};
+
+static void Log_Write_Startup(uint8_t type)
+{
+    struct log_Startup pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
+        time_ms         : millis(),
+        startup_type    : type,
+        command_total   : mission.num_commands()
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+
+    // write all commands to the dataflash as well
+    AP_Mission::Mission_Command cmd;
+    for (uint16_t i = 0; i < mission.num_commands(); i++) {
+        if(mission.read_cmd_from_storage(i,cmd)) {
+            Log_Write_Cmd(cmd);
+        }
+    }
+}
+
+struct PACKED log_Control_Tuning {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t steer_out;
+    int16_t roll;
+    int16_t pitch;
+    int16_t throttle_out;
+    float accel_y;
+};
+
+// Write a control tuning packet. Total length : 22 bytes
+static void Log_Write_Control_Tuning()
+{
+    Vector3f accel = ins.get_accel();
+    struct log_Control_Tuning pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
+        time_ms         : millis(),
+        steer_out       : (int16_t)channel_steer->servo_out,
+        roll            : (int16_t)ahrs.roll_sensor,
+        pitch           : (int16_t)ahrs.pitch_sensor,
+        throttle_out    : (int16_t)channel_throttle->servo_out,
+        accel_y         : accel.y
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+struct PACKED log_Nav_Tuning {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint16_t yaw;
+    float    wp_distance;
+    uint16_t target_bearing_cd;
+    uint16_t nav_bearing_cd;
+    int8_t   throttle;
+};
+
+// Write a navigation tuning packet. Total length : 18 bytes
+static void Log_Write_Nav_Tuning()
+{
+    struct log_Nav_Tuning pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
+        time_ms             : millis(),
+        yaw                 : (uint16_t)ahrs.yaw_sensor,
+        wp_distance         : wp_distance,
+        target_bearing_cd   : (uint16_t)nav_controller->target_bearing_cd(),
+        nav_bearing_cd      : (uint16_t)nav_controller->nav_bearing_cd(),
+        throttle            : (int8_t)(100 * channel_throttle->norm_output())
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+struct PACKED log_Attitude {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t roll;
+    int16_t pitch;
+    uint16_t yaw;
+};
+
+
+// Write an attitude packet
+static void Log_Write_Attitude()
+{
+    struct log_Attitude pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
+        time_ms : millis(),
+        roll    : (int16_t)ahrs.roll_sensor,
+        pitch   : (int16_t)ahrs.pitch_sensor,
+        yaw     : (uint16_t)ahrs.yaw_sensor
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+#if AP_AHRS_NAVEKF_AVAILABLE
+ #if OPTFLOW == ENABLED
+    DataFlash.Log_Write_EKF(ahrs,optflow.enabled());
+ #else
+    DataFlash.Log_Write_EKF(ahrs,false);
+ #endif
+    DataFlash.Log_Write_AHRS2(ahrs);
+#endif
+}
+
+struct PACKED log_Mode {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint8_t mode;
+    uint8_t mode_num;
+};
+
+// Write a mode packet
+static void Log_Write_Mode()
+{
+    struct log_Mode pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_MODE_MSG),
+        time_ms         : millis(),
+        mode            : (uint8_t)control_mode,
+        mode_num        : (uint8_t)control_mode
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+
+struct PACKED log_Sonar {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    float    lateral_accel;
+    uint16_t sonar1_distance;
+    uint16_t sonar2_distance;
+    uint16_t detected_count;
+    int8_t   turn_angle;
+    uint16_t turn_time;
+    uint16_t ground_speed;
+    int8_t   throttle;
+};
+
+// Write a sonar packet
+static void Log_Write_Sonar()
+{
+    uint16_t turn_time = 0;
+    if (obstacle.turn_angle != 0) {
+        turn_time = hal.scheduler->millis() - obstacle.detected_time_ms;
+    }
+    struct log_Sonar pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
+        time_ms         : millis(),
+        lateral_accel   : lateral_acceleration,
+        sonar1_distance : (uint16_t)sonar.distance_cm(0),
+        sonar2_distance : (uint16_t)sonar.distance_cm(1),
+        detected_count  : obstacle.detected_count,
+        turn_angle      : (int8_t)obstacle.turn_angle,
+        turn_time       : turn_time,
+        ground_speed    : (uint16_t)(ground_speed*100),
+        throttle        : (int8_t)(100 * channel_throttle->norm_output())
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
+struct PACKED log_Current {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t throttle_in;
+    int16_t battery_voltage;
+    int16_t current_amps;
+    uint16_t board_voltage;
+    float   current_total;
+};
+
+static void Log_Write_Current()
+{
+    struct log_Current pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
+        time_ms                 : millis(),
+        throttle_in             : channel_throttle->control_in,
+        battery_voltage         : (int16_t)(battery.voltage() * 100.0),
+        current_amps            : (int16_t)(battery.current_amps() * 100.0),
+        board_voltage           : (uint16_t)(hal.analogin->board_voltage()*1000),
+        current_total           : battery.current_total_mah()
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+
+    // also write power status
+    DataFlash.Log_Write_Power();
+}
+
+struct PACKED log_Compass {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t mag_x;
+    int16_t mag_y;
+    int16_t mag_z;
+    int16_t offset_x;
+    int16_t offset_y;
+    int16_t offset_z;
+    int16_t motor_offset_x;
+    int16_t motor_offset_y;
+    int16_t motor_offset_z;
+};
+
+// Write a Compass packet. Total length : 15 bytes
+static void Log_Write_Compass()
+{
+    const Vector3f &mag_offsets = compass.get_offsets();
+    const Vector3f &mag_motor_offsets = compass.get_motor_offsets();
+    const Vector3f &mag = compass.get_field();
+    struct log_Compass pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
+        time_ms         : millis(),
+        mag_x           : (int16_t)mag.x,
+        mag_y           : (int16_t)mag.y,
+        mag_z           : (int16_t)mag.z,
+        offset_x        : (int16_t)mag_offsets.x,
+        offset_y        : (int16_t)mag_offsets.y,
+        offset_z        : (int16_t)mag_offsets.z,
+        motor_offset_x  : (int16_t)mag_motor_offsets.x,
+        motor_offset_y  : (int16_t)mag_motor_offsets.y,
+        motor_offset_z  : (int16_t)mag_motor_offsets.z
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+#if COMPASS_MAX_INSTANCES > 1
+    if (compass.get_count() > 1) {
+        const Vector3f &mag2_offsets = compass.get_offsets(1);
+        const Vector3f &mag2_motor_offsets = compass.get_motor_offsets(1);
+        const Vector3f &mag2 = compass.get_field(1);
+        struct log_Compass pkt2 = {
+            LOG_PACKET_HEADER_INIT(LOG_COMPASS2_MSG),
+            time_ms         : millis(),
+            mag_x           : (int16_t)mag2.x,
+            mag_y           : (int16_t)mag2.y,
+            mag_z           : (int16_t)mag2.z,
+            offset_x        : (int16_t)mag2_offsets.x,
+            offset_y        : (int16_t)mag2_offsets.y,
+            offset_z        : (int16_t)mag2_offsets.z,
+            motor_offset_x  : (int16_t)mag2_motor_offsets.x,
+            motor_offset_y  : (int16_t)mag2_motor_offsets.y,
+            motor_offset_z  : (int16_t)mag2_motor_offsets.z
+        };
+        DataFlash.WriteBlock(&pkt2, sizeof(pkt2));
+    }
+#endif
+#if COMPASS_MAX_INSTANCES > 2
+    if (compass.get_count() > 2) {
+        const Vector3f &mag3_offsets = compass.get_offsets(2);
+        const Vector3f &mag3_motor_offsets = compass.get_motor_offsets(2);
+        const Vector3f &mag3 = compass.get_field(2);
+        struct log_Compass pkt3 = {
+            LOG_PACKET_HEADER_INIT(LOG_COMPASS3_MSG),
+            time_ms         : millis(),
+            mag_x           : (int16_t)mag3.x,
+            mag_y           : (int16_t)mag3.y,
+            mag_z           : (int16_t)mag3.z,
+            offset_x        : (int16_t)mag3_offsets.x,
+            offset_y        : (int16_t)mag3_offsets.y,
+            offset_z        : (int16_t)mag3_offsets.z,
+            motor_offset_x  : (int16_t)mag3_motor_offsets.x,
+            motor_offset_y  : (int16_t)mag3_motor_offsets.y,
+            motor_offset_z  : (int16_t)mag3_motor_offsets.z
+        };
+        DataFlash.WriteBlock(&pkt3, sizeof(pkt3));
+    }
+#endif
+}
+
+
+static void Log_Write_RC(void)
+{
+    DataFlash.Log_Write_RCIN();
+    DataFlash.Log_Write_RCOUT();
+}
+
+static void Log_Write_Baro(void)
+{
+    DataFlash.Log_Write_Baro(barometer);
+}
+
+static const struct LogStructure log_structure[] PROGMEM = {
+    LOG_COMMON_STRUCTURES,
+    { LOG_ATTITUDE_MSG, sizeof(log_Attitude),       
+      "ATT", "IccC",        "TimeMS,Roll,Pitch,Yaw" },
+    { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
+      "PM",  "IIHIhhhBH", "TimeMS,LTime,MLC,gDt,GDx,GDy,GDz,I2CErr,INSErr" },
+    { LOG_STARTUP_MSG, sizeof(log_Startup),         
+      "STRT", "IBH",        "TimeMS,SType,CTot" },
+    { LOG_CTUN_MSG, sizeof(log_Control_Tuning),     
+      "CTUN", "Ihcchf",     "TimeMS,Steer,Roll,Pitch,ThrOut,AccY" },
+    { LOG_NTUN_MSG, sizeof(log_Nav_Tuning),         
+      "NTUN", "IHfHHb",     "TimeMS,Yaw,WpDist,TargBrg,NavBrg,Thr" },
+    { LOG_SONAR_MSG, sizeof(log_Sonar),             
+      "SONR", "IfHHHbHCb",  "TimeMS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
+    { LOG_CURRENT_MSG, sizeof(log_Current),             
+      "CURR", "IhhhHf",     "TimeMS,Thr,Volt,Curr,Vcc,CurrTot" },
+    { LOG_MODE_MSG, sizeof(log_Mode),             
+      "MODE", "IMB",        "TimeMS,Mode,ModeNum" },
+    { LOG_COMPASS_MSG, sizeof(log_Compass),             
+      "MAG", "Ihhhhhhhhh",  "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
+    { LOG_COMPASS2_MSG, sizeof(log_Compass),             
+      "MAG2", "Ihhhhhhhhh",   "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
+    { LOG_STEERING_MSG, sizeof(log_Steering),             
+      "STER", "Iff",   "TimeMS,Demanded,Achieved" },
+};
+
+
+// Read the DataFlash log memory : Packet Parser
+static void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page)
+{
+    cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING
+                             "\nFree RAM: %u\n"),
+                        (unsigned)hal.util->available_memory());
+
+    cliSerial->println_P(PSTR(HAL_BOARD_NAME));
+
+	DataFlash.LogReadProcess(log_num, start_page, end_page, 
+                             print_mode,
+                             cliSerial);
+}
+
+// start a new log
+static void start_logging() 
+{
+    in_mavlink_delay = true;
+    DataFlash.StartNewLog();
+    in_mavlink_delay = false;
+    DataFlash.Log_Write_Message_P(PSTR(FIRMWARE_STRING));
+
+#if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION)
+    DataFlash.Log_Write_Message_P(PSTR("PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION));
+#endif
+
+    // write system identifier as well if available
+    char sysid[40];
+    if (hal.util->get_system_id(sysid)) {
+        DataFlash.Log_Write_Message(sysid);
+    }
+}
+
+#else // LOGGING_ENABLED
+
+// dummy functions
+static void Log_Write_Startup(uint8_t type) {}
+static void Log_Write_Current() {}
+static void Log_Write_Nav_Tuning() {}
+static void Log_Write_Performance() {}
+static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd) {}
+static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
+static void Log_Write_Control_Tuning() {}
+static void Log_Write_Sonar() {}
+static void Log_Write_Mode() {}
+static void Log_Write_Attitude() {}
+static void Log_Write_Compass() {}
+static void start_logging() {}
+static void Log_Write_RC(void) {}
+
+#endif // LOGGING_ENABLED
+

APMrover2/Makefile

+include ../mk/apm.mk

APMrover2/Parameters.h

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef PARAMETERS_H
+#define PARAMETERS_H
+
+#include <AP_Common.h>
+
+// Global parameter class.
+//
+class Parameters {
+public:
+    // The version of the layout as described by the parameter enum.
+    //
+    // When changing the parameter enum in an incompatible fashion, this
+    // value should be incremented by one.
+    //
+    // The increment will prevent old parameters from being used incorrectly
+    // by newer code.
+    //
+    static const uint16_t k_format_version = 16;
+    static const uint16_t k_software_type = 20;
+
+    enum {
+        // Layout version number, always key zero.
+        //
+        k_param_format_version = 0,
+		k_param_software_type,
+
+        // Misc
+        //
+        k_param_log_bitmask_old = 10, // unused
+        k_param_num_resets,
+        k_param_reset_switch_chan,
+        k_param_initial_mode,
+        k_param_scheduler,
+        k_param_relay,
+        k_param_BoardConfig,
+        k_param_pivot_turn_angle,
+        k_param_rc_13,
+        k_param_rc_14,
+
+        // IO pins
+        k_param_rssi_pin = 20,
+        k_param_battery_volt_pin,
+        k_param_battery_curr_pin,
+
+        // braking
+        k_param_braking_percent = 30,
+        k_param_braking_speederr,
+
+        // misc2
+        k_param_log_bitmask = 40,
+        k_param_gps,
+        k_param_serial0_baud,
+        k_param_serial1_baud,
+        k_param_serial2_baud,
+
+
+        // 110: Telemetry control
+        //
+        k_param_gcs0 = 110, // stream rates for uartA
+        k_param_gcs1,       // stream rates for uartC
+        k_param_sysid_this_mav,
+        k_param_sysid_my_gcs,
+        k_param_serial0_baud_old,
+        k_param_serial1_baud_old,
+        k_param_telem_delay,
+        k_param_skip_gyro_cal,
+        k_param_gcs2,       // stream rates for uartD
+        k_param_serial2_baud_old,
+        k_param_serial2_protocol,
+
+        //
+        // 130: Sensor parameters
+        //
+        k_param_compass_enabled = 130,
+        k_param_steering_learn, // unused
+        k_param_NavEKF,  // Extended Kalman Filter Inertial Navigation Group
+        k_param_mission, // mission library
+
+        // 140: battery controls
+        k_param_battery_monitoring = 140,   // deprecated, can be deleted
+        k_param_volt_div_ratio,     // deprecated, can be deleted
+        k_param_curr_amp_per_volt,  // deprecated, can be deleted
+        k_param_input_voltage, // deprecated, can be deleted
+        k_param_pack_capacity,      // deprecated, can be deleted
+        k_param_battery,
+
+        //
+        // 150: Navigation parameters
+        //
+        k_param_crosstrack_gain = 150,
+        k_param_crosstrack_entry_angle,
+        k_param_speed_cruise,
+        k_param_speed_turn_gain,
+        k_param_speed_turn_dist,
+        k_param_ch7_option,
+        k_param_auto_trigger_pin,
+        k_param_auto_kickstart,
+        k_param_turn_circle, // unused
+        k_param_turn_max_g,
+
+        //
+        // 160: Radio settings
+        //
+        k_param_rc_1 = 160,
+        k_param_rc_2,
+        k_param_rc_3,
+        k_param_rc_4,
+        k_param_rc_5,
+        k_param_rc_6,
+        k_param_rc_7,
+        k_param_rc_8,
+
+        // throttle control
+        k_param_throttle_min = 170,
+        k_param_throttle_max,
+        k_param_throttle_cruise,
+        k_param_throttle_slewrate,
+        k_param_throttle_reduction,
+        k_param_skid_steer_in,
+        k_param_skid_steer_out,
+
+        // failsafe control
+        k_param_fs_action = 180,
+        k_param_fs_timeout,
+        k_param_fs_throttle_enabled,
+        k_param_fs_throttle_value,
+        k_param_fs_gcs_enabled,
+
+        // obstacle control
+        k_param_sonar_enabled = 190, // deprecated, can be removed
+        k_param_sonar_old, // unused
+        k_param_sonar_trigger_cm,
+        k_param_sonar_turn_angle,
+        k_param_sonar_turn_time,
+        k_param_sonar2_old, // unused
+        k_param_sonar_debounce,
+        k_param_sonar, // sonar object
+        
+        //
+        // 210: driving modes
+        //
+        k_param_mode_channel = 210,
+        k_param_mode1,
+        k_param_mode2,
+        k_param_mode3,
+        k_param_mode4,
+        k_param_mode5,
+        k_param_mode6,
+        k_param_learn_channel,
+
+        //
+        // 220: Waypoint data
+        //
+        k_param_command_total = 220,    // unused
+        k_param_command_index,          // unused
+        k_param_waypoint_radius,
+
+        //
+        // 230: camera control
+        //
+        k_param_camera,
+        k_param_camera_mount,
+        k_param_camera_mount2,
+
+        //
+        // 240: PID Controllers
+        k_param_pidNavSteer = 230,
+        k_param_pidServoSteer, // unused
+        k_param_pidSpeedThrottle,
+
+        // high RC channels
+        k_param_rc_9 = 235,
+        k_param_rc_10,
+        k_param_rc_11,
+        k_param_rc_12,
+
+        // other objects
+        k_param_sitl = 240,
+        k_param_ahrs,
+        k_param_ins,
+        k_param_compass,
+        k_param_rcmap,
+        k_param_L1_controller,
+        k_param_steerController,
+        k_param_barometer,
+
+        // 254,255: reserved
+        };
+
+    AP_Int16    format_version;
+	AP_Int8	    software_type;
+
+    // Misc
+    //
+    AP_Int32    log_bitmask;
+    AP_Int16    num_resets;
+    AP_Int8	    reset_switch_chan;
+    AP_Int8     initial_mode;
+
+    // IO pins
+    AP_Int8     rssi_pin;
+
+    // braking
+    AP_Int8     braking_percent;
+    AP_Float    braking_speederr;
+
+	// Telemetry control
+	//
+	AP_Int16    sysid_this_mav;
+	AP_Int16    sysid_my_gcs;
+    AP_Int16    serial0_baud;
+    AP_Int16    serial1_baud;
+#if MAVLINK_COMM_NUM_BUFFERS > 2
+    AP_Int16    serial2_baud;
+    AP_Int8     serial2_protocol;
+#endif
+    AP_Int8     telem_delay;
+    AP_Int8     skip_gyro_cal;
+
+    // sensor parameters
+    AP_Int8	    compass_enabled; 
+
+    // navigation parameters
+    //
+    AP_Float    speed_cruise;
+    AP_Int8     speed_turn_gain;
+    AP_Float    speed_turn_dist;    
+    AP_Int8	    ch7_option;
+    AP_Int8     auto_trigger_pin;
+    AP_Float    auto_kickstart;
+    AP_Float    turn_max_g;
+    AP_Int16    pivot_turn_angle;
+
+    // RC channels
+    RC_Channel      rc_1;
+    RC_Channel_aux	rc_2;
+    RC_Channel      rc_3;
+    RC_Channel_aux  rc_4;
+    RC_Channel_aux	rc_5;
+    RC_Channel_aux	rc_6;
+    RC_Channel_aux	rc_7;
+    RC_Channel_aux	rc_8;
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+    RC_Channel_aux rc_9;
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+    RC_Channel_aux rc_10;
+    RC_Channel_aux rc_11;
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+    RC_Channel_aux rc_12;
+    RC_Channel_aux rc_13;
+    RC_Channel_aux rc_14;
+#endif
+
+    // Throttle
+    //
+    AP_Int8     throttle_min;
+    AP_Int8     throttle_max;
+    AP_Int8     throttle_cruise;
+    AP_Int8     throttle_slewrate;
+    AP_Int8     skid_steer_in;
+    AP_Int8     skid_steer_out;
+
+    // failsafe control
+    AP_Int8     fs_action;
+    AP_Float    fs_timeout;
+    AP_Int8     fs_throttle_enabled;
+    AP_Int16    fs_throttle_value;
+	AP_Int8	    fs_gcs_enabled;
+
+    // obstacle control
+    AP_Int16    sonar_trigger_cm;
+    AP_Float    sonar_turn_angle;
+    AP_Float    sonar_turn_time;
+    AP_Int8     sonar_debounce;
+    
+
+    // driving modes
+    //
+    AP_Int8     mode_channel;
+    AP_Int8     mode1;
+    AP_Int8     mode2;
+    AP_Int8     mode3;
+    AP_Int8     mode4;
+    AP_Int8     mode5;
+    AP_Int8     mode6;
+    AP_Int8     learn_channel;
+    
+    // Waypoints
+    //
+    AP_Float    waypoint_radius;
+
+    // PID controllers
+    //
+    PID         pidSpeedThrottle;
+
+    Parameters() :
+        // RC channels
+        rc_1(CH_1),
+        rc_2(CH_2),
+        rc_3(CH_3),
+        rc_4(CH_4),
+        rc_5(CH_5),
+        rc_6(CH_6),
+        rc_7(CH_7),
+        rc_8(CH_8),
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+        rc_9                                    (CH_9),
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+        rc_10                                   (CH_10),
+        rc_11                                   (CH_11),
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+        rc_12                                   (CH_12),
+        rc_13                                   (CH_13),
+        rc_14                                   (CH_14),
+#endif
+
+        // PID controller    initial P        initial I        initial D        initial imax
+        //-----------------------------------------------------------------------------------
+        pidSpeedThrottle    (0.7,             0.2,             0.2,             4000)
+        {}
+};
+
+extern const AP_Param::Info var_info[];
+
+#endif // PARAMETERS_H
+

APMrover2/Steering.pde

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*****************************************
+* Throttle slew limit
+*****************************************/
+static void throttle_slew_limit(int16_t last_throttle)
+{
+    // if slew limit rate is set to zero then do not slew limit
+    if (g.throttle_slewrate && last_throttle != 0) {                   
+        // limit throttle change by the given percentage per second
+        float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
+        // allow a minimum change of 1 PWM per cycle
+        if (temp < 1) {
+            temp = 1;
+        }
+        channel_throttle->radio_out = constrain_int16(channel_throttle->radio_out, last_throttle - temp, last_throttle + temp);
+    }
+}
+
+/*
+  check for triggering of start of auto mode
+ */
+static bool auto_check_trigger(void)
+{
+    // only applies to AUTO mode
+    if (control_mode != AUTO) {
+        return true;
+    }
+
+    // check for user pressing the auto trigger to off
+    if (auto_triggered && g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 1) {
+        gcs_send_text_P(SEVERITY_LOW, PSTR("AUTO triggered off"));
+        auto_triggered = false;
+        return false; 
+    }
+
+    // if already triggered, then return true, so you don't
+    // need to hold the switch down
+    if (auto_triggered) {
+        return true;
+    }
+
+    if (g.auto_trigger_pin == -1 && g.auto_kickstart == 0.0f) {
+        // no trigger configured - let's go!
+        auto_triggered = true;
+        return true;
+    }
+ 
+    if (g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 0) {
+        gcs_send_text_P(SEVERITY_LOW, PSTR("Triggered AUTO with pin"));
+        auto_triggered = true;
+        return true;            
+    }
+
+    if (g.auto_kickstart != 0.0f) {
+        float xaccel = ins.get_accel().x;
+        if (xaccel >= g.auto_kickstart) {
+            gcs_send_text_fmt(PSTR("Triggered AUTO xaccel=%.1f"), xaccel);
+            auto_triggered = true;
+            return true;            
+        }
+    }
+
+    return false;   
+}
+
+/*
+  work out if we are going to use pivot steering
+ */
+static bool use_pivot_steering(void)
+{
+    if (control_mode >= AUTO && g.skid_steer_out && g.pivot_turn_angle != 0) {
+        int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
+        if (abs(bearing_error) > g.pivot_turn_angle) {
+            return true;
+        }
+    }
+    return false;
+}
+
+
+/*
+  calculate the throtte for auto-throttle modes
+ */
+static void calc_throttle(float target_speed)
+{  
+    if (!auto_check_trigger()) {
+        channel_throttle->servo_out = g.throttle_min.get();
+        return;
+    }
+
+    float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
+    int throttle_target = throttle_base + throttle_nudge;  
+
+    /*
+      reduce target speed in proportion to turning rate, up to the
+      SPEED_TURN_GAIN percentage.
+    */
+    float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
+    steer_rate = constrain_float(steer_rate, 0.0, 1.0);
+
+    // use g.speed_turn_gain for a 90 degree turn, and in proportion
+    // for other turn angles
+    int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
+    float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1);
+    float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;
+
+    float reduction = 1.0 - steer_rate*speed_turn_reduction;
+    
+    if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
+        // in auto-modes we reduce speed when approaching waypoints
+        float reduction2 = 1.0 - speed_turn_reduction;
+        if (reduction2 < reduction) {
+            reduction = reduction2;
+        }
+    }
+    
+    // reduce the target speed by the reduction factor
+    target_speed *= reduction;
+
+    groundspeed_error = fabsf(target_speed) - ground_speed; 
+    
+    throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
+
+    // also reduce the throttle by the reduction factor. This gives a
+    // much faster response in turns
+    throttle *= reduction;
+
+    if (in_reverse) {
+        channel_throttle->servo_out = constrain_int16(-throttle, -g.throttle_max, -g.throttle_min);
+    } else {
+        channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min, g.throttle_max);
+    }
+
+    if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
+        // the user has asked to use reverse throttle to brake. Apply
+        // it in proportion to the ground speed error, but only when
+        // our ground speed error is more than BRAKING_SPEEDERR.
+        //
+        // We use a linear gain, with 0 gain at a ground speed error
+        // of braking_speederr, and 100% gain when groundspeed_error
+        // is 2*braking_speederr
+        float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
+        int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
+        channel_throttle->servo_out = constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min);
+
+        // temporarily set us in reverse to allow the PWM setting to
+        // go negative
+        set_reverse(true);
+    }
+    
+    if (use_pivot_steering()) {
+        channel_throttle->servo_out = 0;
+    }
+}
+
+/*****************************************
+ * Calculate desired turn angles (in medium freq loop)
+ *****************************************/
+
+static void calc_lateral_acceleration()
+{
+    switch (control_mode) {
+    case AUTO:
+        nav_controller->update_waypoint(prev_WP, next_WP);
+        break;
+
+    case RTL:
+    case GUIDED:
+    case STEERING:
+        nav_controller->update_waypoint(current_loc, next_WP);
+        break;
+    default:
+        return;
+    }
+
+	// Calculate the required turn of the wheels
+
+    // negative error = left turn
+	// positive error = right turn
+    lateral_acceleration = nav_controller->lateral_acceleration();
+    if (use_pivot_steering()) {
+        int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
+        if (bearing_error > 0) {
+            lateral_acceleration = g.turn_max_g*GRAVITY_MSS;
+        } else {
+            lateral_acceleration = -g.turn_max_g*GRAVITY_MSS;
+        }
+    }
+}
+
+/*
+  calculate steering angle given lateral_acceleration
+ */
+static void calc_nav_steer()
+{
+    // add in obstacle avoidance
+    lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
+
+    // constrain to max G force
+    lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);
+
+    channel_steer->servo_out = steerController.get_steering_out_lat_accel(lateral_acceleration);
+}
+
+/*****************************************
+* Set the flight control servos based on the current calculated values
+*****************************************/
+static void set_servos(void)
+{
+    static int16_t last_throttle;
+
+    // support a separate steering channel
+    RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0));
+
+	if (control_mode == MANUAL || control_mode == LEARNING) {
+        // do a direct pass through of radio values
+        channel_steer->radio_out       = channel_steer->read();
+        channel_throttle->radio_out    = channel_throttle->read();
+        if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
+            // suppress throttle if in failsafe and manual
+            channel_throttle->radio_out = channel_throttle->radio_trim;
+        }
+	} else {       
+        channel_steer->calc_pwm();
+        if (in_reverse) {
+            channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
+                                                          -g.throttle_max,
+                                                          -g.throttle_min);
+        } else {
+            channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
+                                                          g.throttle_min.get(), 
+                                                          g.throttle_max.get());
+        }
+
+        if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
+            // suppress throttle if in failsafe
+            channel_throttle->servo_out = 0;
+        }
+
+        // convert 0 to 100% into PWM
+        channel_throttle->calc_pwm();
+
+        // limit throttle movement speed
+        throttle_slew_limit(last_throttle);
+    }
+
+    // record last throttle before we apply skid steering
+    last_throttle = channel_throttle->radio_out;
+
+    if (g.skid_steer_out) {
+        // convert the two radio_out values to skid steering values
+        /*
+          mixing rule:
+          steering = motor1 - motor2
+          throttle = 0.5*(motor1 + motor2)
+          motor1 = throttle + 0.5*steering
+          motor2 = throttle - 0.5*steering
+        */          
+        float steering_scaled = channel_steer->norm_output();
+        float throttle_scaled = channel_throttle->norm_output();
+        float motor1 = throttle_scaled + 0.5*steering_scaled;
+        float motor2 = throttle_scaled - 0.5*steering_scaled;
+        channel_steer->servo_out = 4500*motor1;
+        channel_throttle->servo_out = 100*motor2;
+        channel_steer->calc_pwm();
+        channel_throttle->calc_pwm();
+    }
+
+
+#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
+	// send values to the PWM timers for output
+	// ----------------------------------------
+    channel_steer->output(); 
+    channel_throttle->output();
+    RC_Channel_aux::output_ch_all();
+#endif
+}
+
+

APMrover2/command_description.txt

+ArduPilotMega 2.0 Commands
+
+Command Structure in bytes
+0 0x00	byte	Command ID
+1 0x01	byte	Parameter 1
+2 0x02	long	Parameter 2
+3 0x03	..
+4 0x04	..
+5 0x05	..
+6 0x06	long	Parameter 3
+7 0x07	..
+8 0x08	..
+9 0x09	..
+10 0x0A	long	Parameter 4
+11 0x0B	..
+11 0x0C	..
+11 0x0D	..
+
+
+
+***********************************
+Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
+***********************************
+Command ID	Name								Parameter 1			Altitude			Latitude			Longitude
+0x10 / 16	MAV_CMD_NAV_WAYPOINT				-					altitude			lat					lon
+
+0x11 / 17	MAV_CMD_NAV_LOITER_UNLIM 		(indefinitely)			altitude			lat					lon
+
+0x12 / 18	MAV_CMD_NAV_LOITER_TURNS			turns				altitude			lat					lon
+
+0x13 / 19	MAV_CMD_NAV_LOITER_TIME				time (seconds*10)	altitude			lat					lon
+
+0x14 / 20	MAV_CMD_NAV_RETURN_TO_LAUNCH		-					altitude			lat					lon
+
+0x15 / 21	MAV_CMD_NAV_LAND					-					altitude			lat					lon
+
+0x16 / 22	MAV_CMD_NAV_TAKEOFF					takeoff pitch		altitude			-					-
+			NOTE:  for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
+
+0x17 / 23	MAV_CMD_NAV_TARGET					-					altitude			lat					lon
+
+
+***********************************
+May Commands - these commands are optional to finish
+Command ID		Name								Parameter 1			Parameter 2			Parameter 3			Parameter 4
+0x70 / 112		MAV_CMD_CONDITION_DELAY				-					-					time (seconds) -
+
+0x71 / 113		MAV_CMD_CONDITION_CHANGE_ALT 		-           		alt (finish)		rate (cm/sec)					-
+													Note: rate must be > 10 cm/sec due to integer math
+
+MAV_CMD_NAV_LAND_OPTIONS			(NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM)
+
+0x72 / 114		MAV_CMD_CONDITION_DISTANCE			-					-					distance (meters)	-
+
+0x71 / 115		MAV_CMD_CONDITION_YAW				angle				speed				direction (-1,1)	rel (1), abs (0)
+
+***********************************
+Unexecuted commands > MAV_CMD_NAV_LAST are dropped when ready for the next command < MAV_CMD_NAV_LAST so plan/queue commands accordingly!
+For example if you had a string of CMD_MAV_CONDITION commands following a 0x10 command that had not finished when the waypoint was
+reached, the unexecuted CMD_MAV_CONDITION and CMD_MAV_DO commands would be skipped and the next command < MAV_CMD_NAV_LAST would be loaded
+***********************************
+Now Commands - these commands are executed once until no more new now commands are available
+
+Command ID		Name						Parameter 1			Parameter 2			Parameter 3			Parameter 4
+0xB1 / 177		MAV_CMD_DO_JUMP				index				-					repeat count		-
+				Note:  The repeat count must be greater than 1 for the command to execute.  Use a repeat count of 1 if you intend a single use.
+
+0XB2 / 178		MAV_CMD_DO_CHANGE_SPEED 	Speed type			Speed (m/s)			Throttle (Percent)	-
+								(0=Airspeed, 1=Ground Speed)	(-1 indicates no change)(-1 indicates no change)
+
+0xB3 / 179		MAV_CMD_DO_SET_HOME			Use current 		altitude			lat					lon
+								(1=use current location, 0=use specified location)
+
+0xB4 / 180		MAV_CMD_DO_SET_PARAMETER	Param number		Param value			(NOT CURRENTLY IMPLEMENTED IN APM)
+
+0xB5 / 181		MAV_CMD_DO_SET_RELAY		Relay number		On/off  (1/0)		-					-
+
+0xB6 / 182		MAV_CMD_DO_REPEAT_RELAY		Relay number		Cycle count			Cycle time (sec)	-
+				Note:  Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
+
+0xB7 / 183		MAV_CMD_DO_SET_SERVO		Servo number (5-8)	On/off  (1/0)		-					-
+
+0xB6 / 184		MAV_CMD_DO_REPEAT_SERVO		Servo number (5-8)	Cycle count			Cycle time (sec)	-
+				Note:  Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
+

APMrover2/commands.pde

+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/* Functions in this file:
+	void set_next_WP(const AP_Mission::Mission_Command& cmd)
+	void set_guided_WP(void)
+	void init_home()
+	void restart_nav()
+************************************************************ 
+*/
+
+
+/*
+ *  set_next_WP - sets the target location the vehicle should fly to
+ */
+static void set_next_WP(const struct Location& loc)
+{
+	// copy the current WP into the OldWP slot
+	// ---------------------------------------
+	prev_WP = next_WP;
+
+	// Load the next_WP slot
+	// ---------------------
+	next_WP = loc;
+
+    // are we already past the waypoint? This happens when we jump
+    // waypoints, and it can cause us to skip a waypoint. If we are
+    // past the waypoint when we start on a leg, then use the current
+    // location as the previous waypoint, to prevent immediately
+    // considering the waypoint complete
+    if (location_passed_point(current_loc, prev_WP, next_WP)) {
+        gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP"));
+        prev_WP = current_loc;
+    }
+
+	// this is handy for the groundstation
+	wp_totalDistance 	= get_distance(current_loc, next_WP);
+	wp_distance 		= wp_totalDistance;
+}
+
+static void set_guided_WP(void)
+{
+	// copy the current location into the OldWP slot
+	// ---------------------------------------
+	prev_WP = current_loc;
+
+	// Load the next_WP slot
+	// ---------------------
+	next_WP = guided_WP;
+
+	// this is handy for the groundstation
+	wp_totalDistance 	= get_distance(current_loc, next_WP);
+	wp_distance 		= wp_totalDistance;
+}
+
+// run this at setup on the ground
+// -------------------------------
+void init_home()
+{
+    if (!have_position) {
+        // we need position information
+        return;
+    }
+
+	gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
+
+    ahrs.set_home(gps.location());
+	home_is_set = true;
+
+	// Save Home to EEPROM
+	mission.write_home_to_storage();
+
+	// Save prev loc
+	// -------------
+	next_WP = prev_WP = home;
+
+	// Load home for a default guided_WP
+	// -------------
+	guided_WP = home;
+}
+
+static void restart_nav()
+{  
+    g.pidSpeedThrottle.reset_I();
+    prev_WP = current_loc;
+    mission.start_or_resume();
+}

APMrover2/commands_logic.pde

+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// forward declarations to make compiler happy
+static void do_nav_wp(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_speed(const AP_Mission::Mission_Command& cmd);
+static void do_set_home(const AP_Mission::Mission_Command& cmd);
+static bool verify_nav_wp(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)) {
+        Log_Write_Cmd(cmd);
+    }
+
+    // exit immediately if not in AUTO mode
+    if (control_mode != AUTO) {
+        return false;
+    }
+
+    gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id);
+
+    // remember the course of our next navigation leg
+    next_navigation_leg_cd = mission.get_next_ground_course_cd(0);
+
+	switch(cmd.id){
+		case MAV_CMD_NAV_WAYPOINT:	// Navigate to Waypoint
+			do_nav_wp(cmd);
+			break;
+
+		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+			do_RTL();
+			break;
+
+        // Conditional commands
+		case MAV_CMD_CONDITION_DELAY:
+			do_wait_delay(cmd);
+			break;
+
+		case MAV_CMD_CONDITION_DISTANCE:
+			do_within_distance(cmd);
+			break;
+
+        // Do commands
+		case MAV_CMD_DO_CHANGE_SPEED:
+			do_change_speed(cmd);
+			break;
+
+		case MAV_CMD_DO_SET_HOME:
+			do_set_home(cmd);
+			break;
+
+    	case MAV_CMD_DO_SET_SERVO:
+            ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
+            break;
+
+    	case MAV_CMD_DO_SET_RELAY:
+            ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
+            break;
+
+    	case MAV_CMD_DO_REPEAT_SERVO:
+            ServoRelayEvents.do_repeat_servo(cmd.content.repeat_servo.channel, cmd.content.repeat_servo.pwm,
+                                             cmd.content.repeat_servo.repeat_count, cmd.content.repeat_servo.cycle_time * 1000.0f);
+            break;
+
+    	case MAV_CMD_DO_REPEAT_RELAY:
+            ServoRelayEvents.do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count,
+                                             cmd.content.repeat_relay.cycle_time * 1000.0f);
+            break;
+
+#if CAMERA == ENABLED
+        case MAV_CMD_DO_CONTROL_VIDEO:                      // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
+            break;
+
+        case MAV_CMD_DO_DIGICAM_CONFIGURE:                  // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
+            break;
+
+        case MAV_CMD_DO_DIGICAM_CONTROL:                    // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
+            do_take_picture();
+            break;
+
+        case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+            camera.set_trigger_distance(cmd.content.cam_trigg_dist.meters);
+            break;
+#endif
+
+#if MOUNT == ENABLED
+		// Sets the region of interest (ROI) for a sensor set or the
+		// vehicle itself. This can then be used by the vehicles control
+		// system to control the vehicle attitude and the attitude of various
+		// devices such as cameras.
+		//    |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
+        case MAV_CMD_DO_SET_ROI:
+            if (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
+                // switch off the camera tracking if enabled
+                if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
+                    camera_mount.set_mode_to_default();
+                }
+            } else {
+                // send the command to the camera mount
+                camera_mount.set_roi_cmd(&cmd.content.location);
+            }
+            break;
+
+		case MAV_CMD_DO_MOUNT_CONFIGURE:	// Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
+			camera_mount.configure_cmd();
+			break;
+
+		case MAV_CMD_DO_MOUNT_CONTROL:		// Mission command to control a camera mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty|
+			camera_mount.control_cmd();
+			break;
+#endif
+
+		default:
+		    // return false for unhandled commands
+		    return false;
+	}
+
+	// if we got this far we must have been successful
+	return true;
+}
+
+// exit_mission - 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()
+{
+    if (control_mode == AUTO) {
+        gcs_send_text_fmt(PSTR("No commands - setting HOLD"));
+        set_mode(HOLD);
+    }
+}
+
+/********************************************************************************/
+// Verify command Handlers
+//      Returns true if command complete
+/********************************************************************************/
+
+static bool verify_command(const AP_Mission::Mission_Command& cmd)
+{
+    // exit immediately if not in AUTO mode
+    // we return true or we will continue to be called by ap-mission
+    if (control_mode != AUTO) {
+        return true;
+    }
+
+	switch(cmd.id) {
+
+		case MAV_CMD_NAV_WAYPOINT:
+			return verify_nav_wp(cmd);
+
+		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+			return verify_RTL();
+
+        case MAV_CMD_CONDITION_DELAY:
+            return verify_wait_delay();
+            break;
+
+        case MAV_CMD_CONDITION_DISTANCE:
+            return verify_within_distance();
+            break;
+
+        default:
+            if (cmd.id > MAV_CMD_CONDITION_LAST) {
+                // this is a command that doesn't require verify
+                return true;
+            }
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command"));
+            return true;
+            break;
+	}
+    return false;
+}
+
+/********************************************************************************/
+//  Nav (Must) commands
+/********************************************************************************/
+
+static void do_RTL(void)
+{
+    prev_WP = current_loc;
+	control_mode 	= RTL;
+	next_WP = home;
+}
+
+static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
+{
+	set_next_WP(cmd.content.location);
+}
+
+/********************************************************************************/
+//  Verify Nav (Must) commands
+/********************************************************************************/
+static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
+{
+    if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
+        gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
+                          (unsigned)cmd.index,
+                          (unsigned)get_distance(current_loc, next_WP));
+        return true;
+    }
+
+    // have we gone past the waypoint?
+    if (location_passed_point(current_loc, prev_WP, next_WP)) {
+        gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
+                          (unsigned)cmd.index,
+                          (unsigned)get_distance(current_loc, next_WP));
+        return true;
+    }
+
+    return false;
+}
+
+static bool verify_RTL()
+{
+	if (wp_distance <= g.waypoint_radius) {
+		gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
+                rtl_complete = true;
+		return true;
+	}
+
+    // have we gone past the waypoint?
+    if (location_passed_point(current_loc, prev_WP, next_WP)) {
+        gcs_send_text_fmt(PSTR("Reached Home dist %um"),
+                          (unsigned)get_distance(current_loc, next_WP));
+        return true;
+    }
+
+    return false;
+}
+
+/********************************************************************************/
+//  Condition (May) commands
+/********************************************************************************/
+
+static void do_wait_delay(const AP_Mission::Mission_Command& cmd)
+{
+	condition_start = millis();
+	condition_value  = cmd.content.delay.seconds * 1000;    // convert seconds to milliseconds
+}
+
+static void do_within_distance(const AP_Mission::Mission_Command& cmd)
+{
+	condition_value  = cmd.content.distance.meters;
+}
+
+/********************************************************************************/
+// Verify Condition (May) commands
+/********************************************************************************/
+
+static bool verify_wait_delay()
+{
+	if ((uint32_t)(millis() - condition_start) > (uint32_t)condition_value){
+		condition_value 	= 0;
+		return true;
+	}
+	return false;
+}
+
+static bool verify_within_distance()
+{
+	if (wp_distance < condition_value){
+		condition_value = 0;
+		return true;
+	}
+	return false;
+}
+
+/********************************************************************************/
+//  Do (Now) commands
+/********************************************************************************/
+
+static void do_change_speed(const AP_Mission::Mission_Command& cmd)
+{
+	switch (cmd.p1)
+	{
+		case 0:
+			if (cmd.content.speed.target_ms > 0) {
+				g.speed_cruise.set(cmd.content.speed.target_ms);
+                gcs_send_text_fmt(PSTR("Cruise speed: %.1f m/s"), g.speed_cruise.get());
+            }
+			break;
+	}
+
+	if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) {
+		g.throttle_cruise.set(cmd.content.speed.throttle_pct);
+        gcs_send_text_fmt(PSTR("Cruise throttle: %.1f"), g.throttle_cruise.get());
+    }
+}
+
+static void do_set_home(const AP_Mission::Mission_Command& cmd)
+{
+	if(cmd.p1 == 1 && have_position) {
+		init_home();
+	} else {
+        ahrs.set_home(cmd.content.location);
+		home_is_set = true;
+	}
+}
+
+// do_take_picture - take a picture with the camera library
+static void do_take_picture()
+{
+#if CAMERA == ENABLED
+    camera.trigger_pic();
+    gcs_send_message(MSG_CAMERA_FEEDBACK);
+    if (should_log(MASK_LOG_CAMERA)) {
+        DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
+    }
+#endif
+}

APMrover2/commands_process.pde

+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// called by update navigation at 10Hz
+// --------------------
+static void update_commands(void)
+{
+    if(control_mode == AUTO) {
+        if(home_is_set == true && mission.num_commands() > 1) {
+            mission.update();
+        }
+    }
+}

APMrover2/compat.h

+
+#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__
+

APMrover2/compat.pde

+
+
+static void delay(uint32_t ms)
+{
+    hal.scheduler->delay(ms);
+}
+
+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();
+}
+

APMrover2/control_modes.pde

+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+static void read_control_switch()
+{
+	
+	uint8_t switchPosition = readSwitch();
+	
+	// If switchPosition = 255 this indicates that the mode control channel input was out of range
+	// If we get this value we do not want to change modes.
+	if(switchPosition == 255) return;
+
+    if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 100) {
+        // only use signals that are less than 0.1s old.
+        return;
+    }
+
+    // we look for changes in the switch position. If the
+    // RST_SWITCH_CH parameter is set, then it is a switch that can be
+    // used to force re-reading of the control switch. This is useful
+    // when returning to the previous mode after a failsafe or fence
+    // breach. This channel is best used on a momentary switch (such
+    // as a spring loaded trainer switch).
+	if (oldSwitchPosition != switchPosition ||
+        (g.reset_switch_chan != 0 && 
+         hal.rcin->read(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
+
+		set_mode((enum mode)modes[switchPosition].get());
+
+		oldSwitchPosition = switchPosition;
+		prev_WP = current_loc;
+
+		// reset speed integrator
+        g.pidSpeedThrottle.reset_I();
+	}
+
+}
+
+static uint8_t readSwitch(void){
+    uint16_t pulsewidth = hal.rcin->read(g.mode_channel - 1);
+	if (pulsewidth <= 900 || pulsewidth >= 2200) 	return 255;	// This is an error condition
+	if (pulsewidth > 1230 && pulsewidth <= 1360) 	return 1;
+	if (pulsewidth > 1360 && pulsewidth <= 1490) 	return 2;
+	if (pulsewidth > 1490 && pulsewidth <= 1620) 	return 3;
+	if (pulsewidth > 1620 && pulsewidth <= 1749) 	return 4;	// Software Manual
+	if (pulsewidth >= 1750) 						return 5;	// Hardware Manual
+	return 0;
+}
+
+static void reset_control_switch()
+{
+	oldSwitchPosition = 0;
+	read_control_switch();
+}
+
+#define CH_7_PWM_TRIGGER 1800
+
+// read at 10 hz
+// set this to your trainer switch
+static void read_trim_switch()
+{
+    switch ((enum ch7_option)g.ch7_option.get()) {
+    case CH7_DO_NOTHING:
+        break;
+    case CH7_SAVE_WP:
+		if (channel_learn->radio_in > CH_7_PWM_TRIGGER) {
+            // switch is engaged
+			ch7_flag = true;
+		} else { // switch is disengaged
+			if (ch7_flag) {
+				ch7_flag = false;
+
+				if (control_mode == MANUAL) {
+                    hal.console->println_P(PSTR("Erasing waypoints"));
+                    // if SW7 is ON in MANUAL = Erase the Flight Plan
+					mission.clear();
+                    if (channel_steer->control_in > 3000) {
+						// if roll is full right store the current location as home
+                        init_home();
+                    }
+					return;
+				} else if (control_mode == LEARNING || control_mode == STEERING) {    
+                    // if SW7 is ON in LEARNING = record the Wp
+
+				    // create new mission command
+				    AP_Mission::Mission_Command cmd = {};
+
+				    // set new waypoint to current location
+				    cmd.content.location = current_loc;
+
+				    // make the new command to a waypoint
+				    cmd.id = MAV_CMD_NAV_WAYPOINT;
+
+				    // save command
+				    if(mission.add_cmd(cmd)) {
+                        hal.console->printf_P(PSTR("Learning waypoint %u"), (unsigned)mission.num_commands());
+				    }
+                } else if (control_mode == AUTO) {    
+                    // if SW7 is ON in AUTO = set to RTL  
+                    set_mode(RTL);
+                }
+            }
+        }
+        break;
+    }
+}
+