From 2d9b230be1ec6264e35688a168fb6915154dba92 Mon Sep 17 00:00:00 2001
From: mirkix <mirkix@gmail.com>
Date: Mon, 18 Jan 2016 20:54:43 +0100
Subject: [PATCH 01/29] AP_RangeFinder: Load .data section for HC-SR04 PRU
 driver used by BBBMINI

This adds .data section loading to the HC-SR04 range finder driver used by
BBBMINI. The firmware is running inside a PRU. It is necessary to develop more
complex driver software inside the PRU.
---
 .../AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp | 20 ++++++++++++++++++-
 .../AP_RangeFinder/AP_RangeFinder_BBB_PRU.h   |  1 +
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp
index 7b11b94e9a..f6867ca6f6 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp
@@ -63,7 +63,7 @@ bool AP_RangeFinder_BBB_PRU::detect(RangeFinder &_ranger, uint8_t instance)
     *ctrl = 0;
     hal.scheduler->delay(1);
 
-    // Load firmware
+    // Load firmware (.text)
     FILE *file = fopen("/lib/firmware/rangefinderprutext.bin", "rb");
     if(file == NULL)
     {
@@ -79,6 +79,24 @@ bool AP_RangeFinder_BBB_PRU::detect(RangeFinder &_ranger, uint8_t instance)
 
     munmap(ram, PRU0_IRAM_SIZE);
 
+    ram = mmap(0, PRU0_DRAM_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, PRU0_DRAM_BASE);
+
+    // Load firmware (.data)
+    file = fopen("/lib/firmware/rangefinderprudata.bin", "rb");
+    if(file == NULL)
+    {
+        result = false;
+    }
+
+    if(fread(ram, PRU0_DRAM_SIZE, 1, file) != 1)
+    {
+        result = false;
+    }
+
+    fclose(file);
+
+    munmap(ram, PRU0_DRAM_SIZE);
+
     // Map PRU RAM
     ram = mmap(0, 0x1000, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, PRU0_DRAM_BASE);
     close(mem_fd);
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h
index c1a381c511..00370a0f08 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h
@@ -11,6 +11,7 @@
 #define PRU0_IRAM_SIZE 0x2000
 
 #define PRU0_DRAM_BASE 0x4a300000
+#define PRU0_DRAM_SIZE 0x2000
 
 struct range {
         uint32_t distance;

From 90d266698de777bf992d9566f8d755ce621a0bf3 Mon Sep 17 00:00:00 2001
From: Julien BERAUD <julien.beraud@parrot.com>
Date: Wed, 20 Jan 2016 15:53:49 +0100
Subject: [PATCH 02/29] Frame_Params: Bebop 2 tuning by Leonard

---
 Tools/Frame_params/Parrot_Bebop2.param | 48 ++++++++++++++++----------
 1 file changed, 29 insertions(+), 19 deletions(-)

diff --git a/Tools/Frame_params/Parrot_Bebop2.param b/Tools/Frame_params/Parrot_Bebop2.param
index 3bd8e75867..538110ba09 100644
--- a/Tools/Frame_params/Parrot_Bebop2.param
+++ b/Tools/Frame_params/Parrot_Bebop2.param
@@ -1,5 +1,7 @@
-#NOTE: Bebop2 parameter defaults (Copter-3.4)
+#NOTE: 17/01/2016 5:01:51 PM Frame : 
 ARMING_CHECK,126
+ACRO_YAW_P,4.5
+ANGLE_MAX,4500
 ATC_ACCEL_P_MAX,220000
 ATC_ACCEL_R_MAX,220000
 ATC_ACCEL_Y_MAX,56000
@@ -8,27 +10,35 @@ BATT_CAPACITY,2700
 COMPASS_EXTERNAL,1
 COMPASS_ORIENT,2
 FRAME,3
-INS_GYRO_FILTER,40
-MOT_THST_EXPO,0.05
-MOT_THST_MAX,1.00
+ATC_RATE_FF_ENAB,1
+ATC_SLEW_YAW,1000
+INS_ACCEL_FILTER,20
+INS_GYRO_FILTER,20
+MOT_CURR_MAX,0
+MOT_SPIN_ARMED,70
+MOT_THR_MIX_MAX,0.5
+MOT_THR_MIX_MIN,0.1
+MOT_THST_BAT_MAX,0
+MOT_THST_BAT_MIN,0
+MOT_THST_EXPO,0.3
+MOT_THST_MAX,1
 MOT_YAW_HEADROOM,200
-POS_XY_P,1
-RATE_PIT_D,0.0022
-RATE_PIT_FILT_HZ,40
-RATE_PIT_I,0.126
+RATE_PIT_D,0.0007353554
+RATE_PIT_FILT_HZ,20
+RATE_PIT_I,0.09863888
 RATE_PIT_IMAX,2000
-RATE_PIT_P,0.126
-RATE_RLL_D,0.0005
-RATE_RLL_FILT_HZ,40
-RATE_RLL_I,0.11
+RATE_PIT_P,0.09863888
+RATE_RLL_D,0.0004876749
+RATE_RLL_FILT_HZ,20
+RATE_RLL_I,0.1183951
 RATE_RLL_IMAX,2000
-RATE_RLL_P,0.11
+RATE_RLL_P,0.1183951
 RATE_YAW_D,0
-RATE_YAW_FILT_HZ,3.0
-RATE_YAW_I,0.103
+RATE_YAW_FILT_HZ,1.001376
+RATE_YAW_I,0.09345812
 RATE_YAW_IMAX,1000
-RATE_YAW_P,1.03
+RATE_YAW_P,0.9345812
+RC_FEEL_RP,50
 STB_PIT_P,15
-STB_RLL_P,10
-STB_YAW_P,12
-THR_MIN,130
+STB_RLL_P,15
+STB_YAW_P,14.25551

From a35c0d48b07d6ee80e518fa1f98a50784faab1c5 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Thu, 21 Jan 2016 10:10:44 -0200
Subject: [PATCH 03/29] waf: ardupilotwaf: rename program to ap_program

Make the rename so that there is no name clashes when defining it as a taskgen
method.
---
 APMrover2/wscript                  | 2 +-
 AntennaTracker/wscript             | 2 +-
 ArduCopter/wscript                 | 2 +-
 ArduPlane/wscript                  | 2 +-
 Tools/CPUInfo/wscript              | 2 +-
 Tools/Replay/wscript               | 2 +-
 Tools/ardupilotwaf/ardupilotwaf.py | 8 ++++----
 7 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/APMrover2/wscript b/APMrover2/wscript
index 8cf10db3aa..9cf8bdb5c8 100644
--- a/APMrover2/wscript
+++ b/APMrover2/wscript
@@ -27,7 +27,7 @@ def build(bld):
         use='mavlink',
     )
 
-    ardupilotwaf.program(
+    ardupilotwaf.ap_program(
         bld,
         program_name='ardurover',
         use=vehicle + '_libs',
diff --git a/AntennaTracker/wscript b/AntennaTracker/wscript
index 04e9268b3b..5dbc583fef 100644
--- a/AntennaTracker/wscript
+++ b/AntennaTracker/wscript
@@ -15,7 +15,7 @@ def build(bld):
         use='mavlink',
     )
 
-    ardupilotwaf.program(
+    ardupilotwaf.ap_program(
         bld,
         program_name='antennatracker',
         use=vehicle + '_libs',
diff --git a/ArduCopter/wscript b/ArduCopter/wscript
index 500b38e9d6..9f126bd87d 100644
--- a/ArduCopter/wscript
+++ b/ArduCopter/wscript
@@ -36,7 +36,7 @@ def build(bld):
         use='mavlink',
     )
 
-    ardupilotwaf.program(
+    ardupilotwaf.ap_program(
         bld,
         program_name='arducopter',
         use=vehicle + '_libs',
diff --git a/ArduPlane/wscript b/ArduPlane/wscript
index cb8ec66820..95689e9417 100644
--- a/ArduPlane/wscript
+++ b/ArduPlane/wscript
@@ -37,7 +37,7 @@ def build(bld):
         use='mavlink',
     )
 
-    ardupilotwaf.program(
+    ardupilotwaf.ap_program(
         bld,
         program_name='arduplane',
         use=vehicle + '_libs',
diff --git a/Tools/CPUInfo/wscript b/Tools/CPUInfo/wscript
index 20ac8eb5bf..58048cebd7 100644
--- a/Tools/CPUInfo/wscript
+++ b/Tools/CPUInfo/wscript
@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.ap_program(
         bld,
         use='ap',
         blddestdir='tools',
diff --git a/Tools/Replay/wscript b/Tools/Replay/wscript
index 20ac8eb5bf..58048cebd7 100644
--- a/Tools/Replay/wscript
+++ b/Tools/Replay/wscript
@@ -4,7 +4,7 @@
 import ardupilotwaf
 
 def build(bld):
-    ardupilotwaf.program(
+    ardupilotwaf.ap_program(
         bld,
         use='ap',
         blddestdir='tools',
diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py
index d0e41b5247..aec7571572 100644
--- a/Tools/ardupilotwaf/ardupilotwaf.py
+++ b/Tools/ardupilotwaf/ardupilotwaf.py
@@ -75,7 +75,7 @@ def get_all_libraries(bld):
     libraries.extend(['AP_HAL', 'AP_HAL_Empty'])
     return libraries
 
-def program(bld, blddestdir='bin',
+def ap_program(bld, blddestdir='bin',
             use_legacy_defines=True,
             program_name=None,
             **kw):
@@ -105,7 +105,7 @@ def program(bld, blddestdir='bin',
 
 def example(bld, **kw):
     kw['blddestdir'] = 'examples'
-    program(bld, **kw)
+    ap_program(bld, **kw)
 
 # NOTE: Code in libraries/ is compiled multiple times. So ensure each
 # compilation is independent by providing different index for each.
@@ -163,7 +163,7 @@ def find_tests(bld, use=[]):
     includes = [bld.srcnode.abspath() + '/tests/']
 
     for f in bld.path.ant_glob(incl='*.cpp'):
-        program(
+        ap_program(
             bld,
             features=features,
             includes=includes,
@@ -181,7 +181,7 @@ def find_benchmarks(bld, use=[]):
     includes = [bld.srcnode.abspath() + '/benchmarks/']
 
     for f in bld.path.ant_glob(incl='*.cpp'):
-        program(
+        ap_program(
             bld,
             features=common_features(bld) + ['gbenchmark'],
             includes=includes,

From cf432e8b3e1de102ee84268cfb6f891b497d6a4e Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Thu, 21 Jan 2016 10:53:08 -0200
Subject: [PATCH 04/29] waf: ardupilotwaf: rename vehicle_stlib to ap_stlib

That function is not only for vehicles.
---
 APMrover2/wscript                  | 2 +-
 AntennaTracker/wscript             | 2 +-
 ArduCopter/wscript                 | 2 +-
 ArduPlane/wscript                  | 2 +-
 Tools/ardupilotwaf/ardupilotwaf.py | 8 ++++----
 wscript                            | 2 +-
 6 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/APMrover2/wscript b/APMrover2/wscript
index 9cf8bdb5c8..119d6be607 100644
--- a/APMrover2/wscript
+++ b/APMrover2/wscript
@@ -5,7 +5,7 @@ import ardupilotwaf
 
 def build(bld):
     vehicle = bld.path.name
-    ardupilotwaf.vehicle_stlib(
+    ardupilotwaf.ap_stlib(
         bld,
         name=vehicle + '_libs',
         vehicle=vehicle,
diff --git a/AntennaTracker/wscript b/AntennaTracker/wscript
index 5dbc583fef..7c48826bd4 100644
--- a/AntennaTracker/wscript
+++ b/AntennaTracker/wscript
@@ -5,7 +5,7 @@ import ardupilotwaf
 
 def build(bld):
     vehicle = bld.path.name
-    ardupilotwaf.vehicle_stlib(
+    ardupilotwaf.ap_stlib(
         bld,
         name=vehicle + '_libs',
         vehicle=vehicle,
diff --git a/ArduCopter/wscript b/ArduCopter/wscript
index 9f126bd87d..e9b634c46d 100644
--- a/ArduCopter/wscript
+++ b/ArduCopter/wscript
@@ -5,7 +5,7 @@ import ardupilotwaf
 
 def build(bld):
     vehicle = bld.path.name
-    ardupilotwaf.vehicle_stlib(
+    ardupilotwaf.ap_stlib(
         bld,
         name=vehicle + '_libs',
         vehicle=vehicle,
diff --git a/ArduPlane/wscript b/ArduPlane/wscript
index 95689e9417..c5f4c18361 100644
--- a/ArduPlane/wscript
+++ b/ArduPlane/wscript
@@ -5,7 +5,7 @@ import ardupilotwaf
 
 def build(bld):
     vehicle = bld.path.name
-    ardupilotwaf.vehicle_stlib(
+    ardupilotwaf.ap_stlib(
         bld,
         name=vehicle + '_libs',
         vehicle=vehicle,
diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py
index aec7571572..ac5036f3c5 100644
--- a/Tools/ardupilotwaf/ardupilotwaf.py
+++ b/Tools/ardupilotwaf/ardupilotwaf.py
@@ -124,13 +124,13 @@ def common_features(bld):
         features.append('static_linking')
     return features
 
-def vehicle_stlib(bld, **kw):
+def ap_stlib(bld, **kw):
     if 'name' not in kw:
-        bld.fatal('Missing name for vehicle_stlib')
+        bld.fatal('Missing name for ap_stlib')
     if 'vehicle' not in kw:
-        bld.fatal('Missing vehicle for vehicle_stlib')
+        bld.fatal('Missing vehicle for ap_stlib')
     if 'libraries' not in kw:
-        bld.fatal('Missing libraries for vehicle_stlib')
+        bld.fatal('Missing libraries for ap_stlib')
 
     sources = []
     libraries = kw['libraries'] + bld.env.AP_LIBRARIES
diff --git a/wscript b/wscript
index 28e331e022..f2d107f8b0 100644
--- a/wscript
+++ b/wscript
@@ -137,7 +137,7 @@ def build(bld):
     # the tools and examples. This is the first step until the
     # dependency on the vehicles is reduced. Later we may consider
     # split into smaller pieces with well defined boundaries.
-    ardupilotwaf.vehicle_stlib(
+    ardupilotwaf.ap_stlib(
         bld,
         name='ap',
         vehicle='UNKNOWN',

From 12cfe222ebb0ad1b912331aa7b68e679dc017b2c Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Thu, 21 Jan 2016 11:30:22 -0200
Subject: [PATCH 05/29] waf: ardupilotwaf: decorate build methods with @conf

Bind functions used in wscripts to build context. Additionally, a new function
is created and also decorated with @conf, common_vehicle_libraries(), which
returns COMMON_VEHICLE_DEPENDENT_LIBRARIES.  This patch is a preparation for
making wscripts use methods bound to the build context instead of using them
directly from ardupilotwaf.
---
 Tools/ardupilotwaf/ardupilotwaf.py | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py
index ac5036f3c5..1d44eec5d2 100644
--- a/Tools/ardupilotwaf/ardupilotwaf.py
+++ b/Tools/ardupilotwaf/ardupilotwaf.py
@@ -3,6 +3,7 @@
 
 from __future__ import print_function
 from waflib import Logs, Options, Utils
+from waflib.Configure import conf
 import os.path
 
 SOURCE_EXTS = [
@@ -63,6 +64,7 @@ IGNORED_AP_LIBRARIES = [
     'GCS_Console',
 ]
 
+@conf
 def get_all_libraries(bld):
     libraries = []
     for lib_node in bld.srcnode.ant_glob('libraries/*', dir=True):
@@ -75,6 +77,11 @@ def get_all_libraries(bld):
     libraries.extend(['AP_HAL', 'AP_HAL_Empty'])
     return libraries
 
+@conf
+def common_vehicle_libraries(bld):
+    return COMMON_VEHICLE_DEPENDENT_LIBRARIES
+
+@conf
 def ap_program(bld, blddestdir='bin',
             use_legacy_defines=True,
             program_name=None,
@@ -103,6 +110,7 @@ def ap_program(bld, blddestdir='bin',
         **kw
     )
 
+@conf
 def example(bld, **kw):
     kw['blddestdir'] = 'examples'
     ap_program(bld, **kw)
@@ -124,6 +132,7 @@ def common_features(bld):
         features.append('static_linking')
     return features
 
+@conf
 def ap_stlib(bld, **kw):
     if 'name' not in kw:
         bld.fatal('Missing name for ap_stlib')
@@ -149,6 +158,7 @@ def ap_stlib(bld, **kw):
 
     bld.stlib(**kw)
 
+@conf
 def find_tests(bld, use=[]):
     if not bld.env.HAS_GTEST:
         return
@@ -174,6 +184,7 @@ def find_tests(bld, use=[]):
             use_legacy_defines=False,
         )
 
+@conf
 def find_benchmarks(bld, use=[]):
     if not bld.env.HAS_GBENCHMARK:
         return

From a89a1a8a8a396fc979d0cf52d2a4e1a5886cfb72 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Thu, 21 Jan 2016 11:47:10 -0200
Subject: [PATCH 06/29] waf: use ardupilotwaf as a Waf tool

That will make it possible to replace calls of the form
`ardupilotwaf.<method>(bld, <keyword-args...>)` with
`bld.<method>(<keyword-args...>)` in the wscripts.

Advantages of that approach:
 - there is no need to import ardupilotwaf in every single wscript
 - it follows the same standard used by c and cxx tools (like bld.program,
   bld.stlib etc)
 - semantically, ap_program, ap_stlib, example etc are all build related
   methods, so it makes sense to bind them to the build context
 - from the wscripts' perspective, the code is cleaner, since ardupilotwaf,
   which is not specific to just build contexts, isn't *explictly* used
---
 wscript | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/wscript b/wscript
index f2d107f8b0..f8f3105fe2 100644
--- a/wscript
+++ b/wscript
@@ -117,6 +117,7 @@ def list_boards(ctx):
     print(*boards.get_boards_names())
 
 def build(bld):
+    bld.load('ardupilotwaf')
     bld.load('gtest')
 
     #generate mavlink headers
@@ -137,11 +138,10 @@ def build(bld):
     # the tools and examples. This is the first step until the
     # dependency on the vehicles is reduced. Later we may consider
     # split into smaller pieces with well defined boundaries.
-    ardupilotwaf.ap_stlib(
-        bld,
+    bld.ap_stlib(
         name='ap',
         vehicle='UNKNOWN',
-        libraries=ardupilotwaf.get_all_libraries(bld),
+        libraries=bld.get_all_libraries(),
         use='mavlink',
     )
     # TODO: Currently each vehicle also generate its own copy of the

From e9d3dc9e725efa9ef7ac45424e3676bebc40b074 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Thu, 21 Jan 2016 12:04:51 -0200
Subject: [PATCH 07/29] waf: vehicles and antennatracker: use methods from bld

Instead of from ardupilotwaf.
---
 APMrover2/wscript      | 10 +++-------
 AntennaTracker/wscript | 10 +++-------
 ArduCopter/wscript     | 10 +++-------
 ArduPlane/wscript      | 10 +++-------
 4 files changed, 12 insertions(+), 28 deletions(-)

diff --git a/APMrover2/wscript b/APMrover2/wscript
index 119d6be607..ab334221a1 100644
--- a/APMrover2/wscript
+++ b/APMrover2/wscript
@@ -1,15 +1,12 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
     vehicle = bld.path.name
-    ardupilotwaf.ap_stlib(
-        bld,
+    bld.ap_stlib(
         name=vehicle + '_libs',
         vehicle=vehicle,
-        libraries=ardupilotwaf.COMMON_VEHICLE_DEPENDENT_LIBRARIES + [
+        libraries=bld.common_vehicle_libraries() + [
             'APM_Control',
             'AP_Arming',
             'AP_Camera',
@@ -27,8 +24,7 @@ def build(bld):
         use='mavlink',
     )
 
-    ardupilotwaf.ap_program(
-        bld,
+    bld.ap_program(
         program_name='ardurover',
         use=vehicle + '_libs',
     )
diff --git a/AntennaTracker/wscript b/AntennaTracker/wscript
index 7c48826bd4..090cf60235 100644
--- a/AntennaTracker/wscript
+++ b/AntennaTracker/wscript
@@ -1,22 +1,18 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
     vehicle = bld.path.name
-    ardupilotwaf.ap_stlib(
-        bld,
+    bld.ap_stlib(
         name=vehicle + '_libs',
         vehicle=vehicle,
-        libraries=ardupilotwaf.COMMON_VEHICLE_DEPENDENT_LIBRARIES + [
+        libraries=bld.common_vehicle_libraries() + [
             'PID',
         ],
         use='mavlink',
     )
 
-    ardupilotwaf.ap_program(
-        bld,
+    bld.ap_program(
         program_name='antennatracker',
         use=vehicle + '_libs',
     )
diff --git a/ArduCopter/wscript b/ArduCopter/wscript
index e9b634c46d..e024c719ba 100644
--- a/ArduCopter/wscript
+++ b/ArduCopter/wscript
@@ -1,15 +1,12 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
     vehicle = bld.path.name
-    ardupilotwaf.ap_stlib(
-        bld,
+    bld.ap_stlib(
         name=vehicle + '_libs',
         vehicle=vehicle,
-        libraries=ardupilotwaf.COMMON_VEHICLE_DEPENDENT_LIBRARIES + [
+        libraries=bld.common_vehicle_libraries() + [
             'AP_ADSB',
             'AC_AttitudeControl',
             'AC_Fence',
@@ -36,8 +33,7 @@ def build(bld):
         use='mavlink',
     )
 
-    ardupilotwaf.ap_program(
-        bld,
+    bld.ap_program(
         program_name='arducopter',
         use=vehicle + '_libs',
     )
diff --git a/ArduPlane/wscript b/ArduPlane/wscript
index c5f4c18361..ddfc19b7f3 100644
--- a/ArduPlane/wscript
+++ b/ArduPlane/wscript
@@ -1,15 +1,12 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
     vehicle = bld.path.name
-    ardupilotwaf.ap_stlib(
-        bld,
+    bld.ap_stlib(
         name=vehicle + '_libs',
         vehicle=vehicle,
-        libraries=ardupilotwaf.COMMON_VEHICLE_DEPENDENT_LIBRARIES + [
+        libraries=bld.common_vehicle_libraries() + [
             'APM_Control',
             'APM_OBC',
             'AP_ADSB',
@@ -37,8 +34,7 @@ def build(bld):
         use='mavlink',
     )
 
-    ardupilotwaf.ap_program(
-        bld,
+    bld.ap_program(
         program_name='arduplane',
         use=vehicle + '_libs',
     )

From 3d2249039790eaa34fbf333d9674555ac0499658 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Thu, 21 Jan 2016 20:15:18 -0200
Subject: [PATCH 08/29] waf: examples: use methods from bld instead of
 ardupilotwaf

---
 Tools/Hello/wscript                                          | 5 +----
 libraries/AC_PID/examples/AC_PID_test/wscript                | 5 +----
 libraries/AP_ADC/examples/AP_ADC_test/wscript                | 5 +----
 libraries/AP_AHRS/examples/AHRS_Test/wscript                 | 5 +----
 libraries/AP_Airspeed/examples/Airspeed/wscript              | 5 +----
 libraries/AP_Baro/examples/BARO_generic/wscript              | 5 +----
 .../AP_BattMonitor/examples/AP_BattMonitor_test/wscript      | 5 +----
 libraries/AP_Common/examples/AP_Common/wscript               | 5 +----
 libraries/AP_Compass/examples/AP_Compass_test/wscript        | 5 +----
 .../AP_Declination/examples/AP_Declination_test/wscript      | 5 +----
 libraries/AP_GPS/examples/GPS_AUTO_test/wscript              | 5 +----
 libraries/AP_GPS/examples/GPS_UBLOX_passthrough/wscript      | 5 +----
 libraries/AP_HAL/examples/AnalogIn/wscript                   | 5 +----
 libraries/AP_HAL/examples/Printf/wscript                     | 5 +----
 libraries/AP_HAL/examples/RCInput/wscript                    | 5 +----
 libraries/AP_HAL/examples/RCInputToRCOutput/wscript          | 5 +----
 libraries/AP_HAL/examples/RCOutput/wscript                   | 5 +----
 libraries/AP_HAL/examples/Storage/wscript                    | 5 +----
 libraries/AP_HAL/examples/UART_test/wscript                  | 5 +----
 libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript         | 5 +----
 libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript          | 5 +----
 libraries/AP_HAL_AVR/examples/Blink/wscript                  | 5 +----
 libraries/AP_HAL_AVR/examples/Console/wscript                | 5 +----
 libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript     | 5 +----
 libraries/AP_HAL_AVR/examples/LCDTest/wscript                | 5 +----
 libraries/AP_HAL_AVR/examples/RCInputTest/wscript            | 5 +----
 libraries/AP_HAL_AVR/examples/RCJitterTest/wscript           | 5 +----
 libraries/AP_HAL_AVR/examples/RCPassthroughTest/wscript      | 5 +----
 libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript      | 5 +----
 libraries/AP_HAL_AVR/examples/Scheduler/wscript              | 5 +----
 libraries/AP_HAL_AVR/examples/Semaphore/wscript              | 5 +----
 libraries/AP_HAL_AVR/examples/Storage/wscript                | 5 +----
 libraries/AP_HAL_AVR/examples/UARTDriver/wscript             | 5 +----
 libraries/AP_HAL_AVR/examples/UtilityStringTest/wscript      | 5 +----
 .../AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/wscript     | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript          | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript             | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/Console/wscript           | 5 +----
 .../AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/wscript      | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript           | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript         | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript         | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript         | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript           | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript        | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript | 5 +----
 libraries/AP_HAL_FLYMAPLE/examples/empty_example/wscript     | 5 +----
 libraries/AP_HAL_Linux/examples/BusTest/wscript              | 5 +----
 libraries/AP_HAL_PX4/examples/simple/wscript                 | 5 +----
 libraries/AP_InertialSensor/examples/INS_generic/wscript     | 5 +----
 libraries/AP_InertialSensor/examples/VibTest/wscript         | 5 +----
 libraries/AP_Math/examples/eulers/wscript                    | 5 +----
 libraries/AP_Math/examples/location/wscript                  | 5 +----
 libraries/AP_Math/examples/polygon/wscript                   | 5 +----
 libraries/AP_Math/examples/rotations/wscript                 | 5 +----
 libraries/AP_Mission/examples/AP_Mission_test/wscript        | 5 +----
 libraries/AP_Motors/examples/AP_Motors_Time_test/wscript     | 5 +----
 libraries/AP_Motors/examples/AP_Motors_test/wscript          | 5 +----
 libraries/AP_Mount/examples/trivial_AP_Mount/wscript         | 5 +----
 libraries/AP_Notify/examples/AP_Notify_test/wscript          | 5 +----
 libraries/AP_Notify/examples/ToshibaLED_test/wscript         | 5 +----
 .../AP_OpticalFlow/examples/AP_OpticalFlow_test/wscript      | 5 +----
 libraries/AP_Parachute/examples/AP_Parachute_test/wscript    | 5 +----
 libraries/AP_RangeFinder/examples/RFIND_test/wscript         | 5 +----
 libraries/AP_Scheduler/examples/Scheduler_test/wscript       | 5 +----
 libraries/DataFlash/examples/DataFlash_test/wscript          | 5 +----
 libraries/Filter/examples/Derivative/wscript                 | 5 +----
 libraries/Filter/examples/Filter/wscript                     | 5 +----
 libraries/Filter/examples/LowPassFilter/wscript              | 5 +----
 libraries/Filter/examples/LowPassFilter2p/wscript            | 5 +----
 libraries/GCS_Console/examples/Console/wscript               | 5 +----
 libraries/GCS_MAVLink/examples/routing/wscript               | 5 +----
 libraries/PID/examples/pid/wscript                           | 5 +----
 libraries/RC_Channel/examples/RC_Channel/wscript             | 5 +----
 libraries/RC_Channel/examples/RC_UART/wscript                | 5 +----
 libraries/StorageManager/examples/StorageTest/wscript        | 5 +----
 77 files changed, 77 insertions(+), 308 deletions(-)

diff --git a/Tools/Hello/wscript b/Tools/Hello/wscript
index 4dafd0b036..36590ce02a 100644
--- a/Tools/Hello/wscript
+++ b/Tools/Hello/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AC_PID/examples/AC_PID_test/wscript b/libraries/AC_PID/examples/AC_PID_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AC_PID/examples/AC_PID_test/wscript
+++ b/libraries/AC_PID/examples/AC_PID_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_ADC/examples/AP_ADC_test/wscript b/libraries/AP_ADC/examples/AP_ADC_test/wscript
index 5af31fa1ae..15d4b016ec 100644
--- a/libraries/AP_ADC/examples/AP_ADC_test/wscript
+++ b/libraries/AP_ADC/examples/AP_ADC_test/wscript
@@ -1,13 +1,10 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
     if bld.env.BOARD in ['sitl']:
         return
 
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_AHRS/examples/AHRS_Test/wscript b/libraries/AP_AHRS/examples/AHRS_Test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_AHRS/examples/AHRS_Test/wscript
+++ b/libraries/AP_AHRS/examples/AHRS_Test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Airspeed/examples/Airspeed/wscript b/libraries/AP_Airspeed/examples/Airspeed/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Airspeed/examples/Airspeed/wscript
+++ b/libraries/AP_Airspeed/examples/Airspeed/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Baro/examples/BARO_generic/wscript b/libraries/AP_Baro/examples/BARO_generic/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Baro/examples/BARO_generic/wscript
+++ b/libraries/AP_Baro/examples/BARO_generic/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/wscript b/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/wscript
+++ b/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Common/examples/AP_Common/wscript b/libraries/AP_Common/examples/AP_Common/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Common/examples/AP_Common/wscript
+++ b/libraries/AP_Common/examples/AP_Common/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Compass/examples/AP_Compass_test/wscript b/libraries/AP_Compass/examples/AP_Compass_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Compass/examples/AP_Compass_test/wscript
+++ b/libraries/AP_Compass/examples/AP_Compass_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Declination/examples/AP_Declination_test/wscript b/libraries/AP_Declination/examples/AP_Declination_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Declination/examples/AP_Declination_test/wscript
+++ b/libraries/AP_Declination/examples/AP_Declination_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/wscript b/libraries/AP_GPS/examples/GPS_AUTO_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_GPS/examples/GPS_AUTO_test/wscript
+++ b/libraries/AP_GPS/examples/GPS_AUTO_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_GPS/examples/GPS_UBLOX_passthrough/wscript b/libraries/AP_GPS/examples/GPS_UBLOX_passthrough/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_GPS/examples/GPS_UBLOX_passthrough/wscript
+++ b/libraries/AP_GPS/examples/GPS_UBLOX_passthrough/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/AnalogIn/wscript b/libraries/AP_HAL/examples/AnalogIn/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL/examples/AnalogIn/wscript
+++ b/libraries/AP_HAL/examples/AnalogIn/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/Printf/wscript b/libraries/AP_HAL/examples/Printf/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL/examples/Printf/wscript
+++ b/libraries/AP_HAL/examples/Printf/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/RCInput/wscript b/libraries/AP_HAL/examples/RCInput/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL/examples/RCInput/wscript
+++ b/libraries/AP_HAL/examples/RCInput/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/RCInputToRCOutput/wscript b/libraries/AP_HAL/examples/RCInputToRCOutput/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL/examples/RCInputToRCOutput/wscript
+++ b/libraries/AP_HAL/examples/RCInputToRCOutput/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/RCOutput/wscript b/libraries/AP_HAL/examples/RCOutput/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL/examples/RCOutput/wscript
+++ b/libraries/AP_HAL/examples/RCOutput/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/Storage/wscript b/libraries/AP_HAL/examples/Storage/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL/examples/Storage/wscript
+++ b/libraries/AP_HAL/examples/Storage/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/UART_test/wscript b/libraries/AP_HAL/examples/UART_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL/examples/UART_test/wscript
+++ b/libraries/AP_HAL/examples/UART_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript b/libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript
+++ b/libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript b/libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript
+++ b/libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/Blink/wscript b/libraries/AP_HAL_AVR/examples/Blink/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/Blink/wscript
+++ b/libraries/AP_HAL_AVR/examples/Blink/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/Console/wscript b/libraries/AP_HAL_AVR/examples/Console/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/Console/wscript
+++ b/libraries/AP_HAL_AVR/examples/Console/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript b/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript
+++ b/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/LCDTest/wscript b/libraries/AP_HAL_AVR/examples/LCDTest/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/LCDTest/wscript
+++ b/libraries/AP_HAL_AVR/examples/LCDTest/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/RCInputTest/wscript b/libraries/AP_HAL_AVR/examples/RCInputTest/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/RCInputTest/wscript
+++ b/libraries/AP_HAL_AVR/examples/RCInputTest/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/RCJitterTest/wscript b/libraries/AP_HAL_AVR/examples/RCJitterTest/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/RCJitterTest/wscript
+++ b/libraries/AP_HAL_AVR/examples/RCJitterTest/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/RCPassthroughTest/wscript b/libraries/AP_HAL_AVR/examples/RCPassthroughTest/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/RCPassthroughTest/wscript
+++ b/libraries/AP_HAL_AVR/examples/RCPassthroughTest/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript b/libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript
+++ b/libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/Scheduler/wscript b/libraries/AP_HAL_AVR/examples/Scheduler/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/Scheduler/wscript
+++ b/libraries/AP_HAL_AVR/examples/Scheduler/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/Semaphore/wscript b/libraries/AP_HAL_AVR/examples/Semaphore/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/Semaphore/wscript
+++ b/libraries/AP_HAL_AVR/examples/Semaphore/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/Storage/wscript b/libraries/AP_HAL_AVR/examples/Storage/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/Storage/wscript
+++ b/libraries/AP_HAL_AVR/examples/Storage/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/UARTDriver/wscript b/libraries/AP_HAL_AVR/examples/UARTDriver/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/UARTDriver/wscript
+++ b/libraries/AP_HAL_AVR/examples/UARTDriver/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/UtilityStringTest/wscript b/libraries/AP_HAL_AVR/examples/UtilityStringTest/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_AVR/examples/UtilityStringTest/wscript
+++ b/libraries/AP_HAL_AVR/examples/UtilityStringTest/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/wscript b/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript b/libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript b/libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Console/wscript b/libraries/AP_HAL_FLYMAPLE/examples/Console/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/Console/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/Console/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/wscript b/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript b/libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript b/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript b/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript b/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript b/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript b/libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript b/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript b/libraries/AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/empty_example/wscript b/libraries/AP_HAL_FLYMAPLE/examples/empty_example/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/empty_example/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/empty_example/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_Linux/examples/BusTest/wscript b/libraries/AP_HAL_Linux/examples/BusTest/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_Linux/examples/BusTest/wscript
+++ b/libraries/AP_HAL_Linux/examples/BusTest/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_PX4/examples/simple/wscript b/libraries/AP_HAL_PX4/examples/simple/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_HAL_PX4/examples/simple/wscript
+++ b/libraries/AP_HAL_PX4/examples/simple/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_InertialSensor/examples/INS_generic/wscript b/libraries/AP_InertialSensor/examples/INS_generic/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_InertialSensor/examples/INS_generic/wscript
+++ b/libraries/AP_InertialSensor/examples/INS_generic/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_InertialSensor/examples/VibTest/wscript b/libraries/AP_InertialSensor/examples/VibTest/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_InertialSensor/examples/VibTest/wscript
+++ b/libraries/AP_InertialSensor/examples/VibTest/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Math/examples/eulers/wscript b/libraries/AP_Math/examples/eulers/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Math/examples/eulers/wscript
+++ b/libraries/AP_Math/examples/eulers/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Math/examples/location/wscript b/libraries/AP_Math/examples/location/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Math/examples/location/wscript
+++ b/libraries/AP_Math/examples/location/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Math/examples/polygon/wscript b/libraries/AP_Math/examples/polygon/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Math/examples/polygon/wscript
+++ b/libraries/AP_Math/examples/polygon/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Math/examples/rotations/wscript b/libraries/AP_Math/examples/rotations/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Math/examples/rotations/wscript
+++ b/libraries/AP_Math/examples/rotations/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Mission/examples/AP_Mission_test/wscript b/libraries/AP_Mission/examples/AP_Mission_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Mission/examples/AP_Mission_test/wscript
+++ b/libraries/AP_Mission/examples/AP_Mission_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Motors/examples/AP_Motors_Time_test/wscript b/libraries/AP_Motors/examples/AP_Motors_Time_test/wscript
index 4d5bbad799..20cab3129f 100644
--- a/libraries/AP_Motors/examples/AP_Motors_Time_test/wscript
+++ b/libraries/AP_Motors/examples/AP_Motors_Time_test/wscript
@@ -1,13 +1,10 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
     # TODO: Test code doesn't build. Fix or delete the test.
     return
 
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Motors/examples/AP_Motors_test/wscript b/libraries/AP_Motors/examples/AP_Motors_test/wscript
index 4d5bbad799..20cab3129f 100644
--- a/libraries/AP_Motors/examples/AP_Motors_test/wscript
+++ b/libraries/AP_Motors/examples/AP_Motors_test/wscript
@@ -1,13 +1,10 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
     # TODO: Test code doesn't build. Fix or delete the test.
     return
 
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Mount/examples/trivial_AP_Mount/wscript b/libraries/AP_Mount/examples/trivial_AP_Mount/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Mount/examples/trivial_AP_Mount/wscript
+++ b/libraries/AP_Mount/examples/trivial_AP_Mount/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Notify/examples/AP_Notify_test/wscript b/libraries/AP_Notify/examples/AP_Notify_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Notify/examples/AP_Notify_test/wscript
+++ b/libraries/AP_Notify/examples/AP_Notify_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Notify/examples/ToshibaLED_test/wscript b/libraries/AP_Notify/examples/ToshibaLED_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Notify/examples/ToshibaLED_test/wscript
+++ b/libraries/AP_Notify/examples/ToshibaLED_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/wscript b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/wscript
+++ b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Parachute/examples/AP_Parachute_test/wscript b/libraries/AP_Parachute/examples/AP_Parachute_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Parachute/examples/AP_Parachute_test/wscript
+++ b/libraries/AP_Parachute/examples/AP_Parachute_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_RangeFinder/examples/RFIND_test/wscript b/libraries/AP_RangeFinder/examples/RFIND_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_RangeFinder/examples/RFIND_test/wscript
+++ b/libraries/AP_RangeFinder/examples/RFIND_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/AP_Scheduler/examples/Scheduler_test/wscript b/libraries/AP_Scheduler/examples/Scheduler_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/AP_Scheduler/examples/Scheduler_test/wscript
+++ b/libraries/AP_Scheduler/examples/Scheduler_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/DataFlash/examples/DataFlash_test/wscript b/libraries/DataFlash/examples/DataFlash_test/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/DataFlash/examples/DataFlash_test/wscript
+++ b/libraries/DataFlash/examples/DataFlash_test/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/Filter/examples/Derivative/wscript b/libraries/Filter/examples/Derivative/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/Filter/examples/Derivative/wscript
+++ b/libraries/Filter/examples/Derivative/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/Filter/examples/Filter/wscript b/libraries/Filter/examples/Filter/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/Filter/examples/Filter/wscript
+++ b/libraries/Filter/examples/Filter/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/Filter/examples/LowPassFilter/wscript b/libraries/Filter/examples/LowPassFilter/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/Filter/examples/LowPassFilter/wscript
+++ b/libraries/Filter/examples/LowPassFilter/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/Filter/examples/LowPassFilter2p/wscript b/libraries/Filter/examples/LowPassFilter2p/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/Filter/examples/LowPassFilter2p/wscript
+++ b/libraries/Filter/examples/LowPassFilter2p/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/GCS_Console/examples/Console/wscript b/libraries/GCS_Console/examples/Console/wscript
index 4d5bbad799..20cab3129f 100644
--- a/libraries/GCS_Console/examples/Console/wscript
+++ b/libraries/GCS_Console/examples/Console/wscript
@@ -1,13 +1,10 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
     # TODO: Test code doesn't build. Fix or delete the test.
     return
 
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/GCS_MAVLink/examples/routing/wscript b/libraries/GCS_MAVLink/examples/routing/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/GCS_MAVLink/examples/routing/wscript
+++ b/libraries/GCS_MAVLink/examples/routing/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/PID/examples/pid/wscript b/libraries/PID/examples/pid/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/PID/examples/pid/wscript
+++ b/libraries/PID/examples/pid/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/RC_Channel/examples/RC_Channel/wscript b/libraries/RC_Channel/examples/RC_Channel/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/RC_Channel/examples/RC_Channel/wscript
+++ b/libraries/RC_Channel/examples/RC_Channel/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/RC_Channel/examples/RC_UART/wscript b/libraries/RC_Channel/examples/RC_UART/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/RC_Channel/examples/RC_UART/wscript
+++ b/libraries/RC_Channel/examples/RC_UART/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )
diff --git a/libraries/StorageManager/examples/StorageTest/wscript b/libraries/StorageManager/examples/StorageTest/wscript
index 4dafd0b036..36590ce02a 100644
--- a/libraries/StorageManager/examples/StorageTest/wscript
+++ b/libraries/StorageManager/examples/StorageTest/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.example(
-        bld,
+    bld.example(
         use='ap',
     )

From a2d2661765568b046f5dad1a64ca60f5e080c7f6 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Thu, 21 Jan 2016 20:34:14 -0200
Subject: [PATCH 09/29] waf: use methods from bld instead of ardupilotwaf for
 the remaining

---
 Tools/CPUInfo/wscript                     | 5 +----
 Tools/Replay/wscript                      | 5 +----
 libraries/AP_HAL_Linux/benchmarks/wscript | 5 +----
 libraries/AP_Math/benchmarks/wscript      | 5 +----
 libraries/AP_Math/tests/wscript           | 5 +----
 5 files changed, 5 insertions(+), 20 deletions(-)

diff --git a/Tools/CPUInfo/wscript b/Tools/CPUInfo/wscript
index 58048cebd7..914f3ea59e 100644
--- a/Tools/CPUInfo/wscript
+++ b/Tools/CPUInfo/wscript
@@ -1,11 +1,8 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.ap_program(
-        bld,
+    bld.ap_program(
         use='ap',
         blddestdir='tools',
     )
diff --git a/Tools/Replay/wscript b/Tools/Replay/wscript
index 58048cebd7..914f3ea59e 100644
--- a/Tools/Replay/wscript
+++ b/Tools/Replay/wscript
@@ -1,11 +1,8 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.ap_program(
-        bld,
+    bld.ap_program(
         use='ap',
         blddestdir='tools',
     )
diff --git a/libraries/AP_HAL_Linux/benchmarks/wscript b/libraries/AP_HAL_Linux/benchmarks/wscript
index 37a6f3f1f8..edffe3938d 100644
--- a/libraries/AP_HAL_Linux/benchmarks/wscript
+++ b/libraries/AP_HAL_Linux/benchmarks/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.find_benchmarks(
-        bld,
+    bld.find_benchmarks(
         use='ap',
     )
diff --git a/libraries/AP_Math/benchmarks/wscript b/libraries/AP_Math/benchmarks/wscript
index 37a6f3f1f8..edffe3938d 100644
--- a/libraries/AP_Math/benchmarks/wscript
+++ b/libraries/AP_Math/benchmarks/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.find_benchmarks(
-        bld,
+    bld.find_benchmarks(
         use='ap',
     )
diff --git a/libraries/AP_Math/tests/wscript b/libraries/AP_Math/tests/wscript
index e4d42f80d0..17479ab441 100644
--- a/libraries/AP_Math/tests/wscript
+++ b/libraries/AP_Math/tests/wscript
@@ -1,10 +1,7 @@
 #!/usr/bin/env python
 # encoding: utf-8
 
-import ardupilotwaf
-
 def build(bld):
-    ardupilotwaf.find_tests(
-        bld,
+    bld.find_tests(
         use='ap',
     )

From c2e3f05dbfe7fd288a3ffd077861b9dd36b0f1e8 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Fri, 22 Jan 2016 21:00:13 +0000
Subject: [PATCH 10/29] waf: ardupilotwaf: prefix build context methods with
 ap_

It helps to distinguish between things from waf and things from ardupilotwaf.
---
 APMrover2/wscript                                      |  2 +-
 AntennaTracker/wscript                                 |  2 +-
 ArduCopter/wscript                                     |  2 +-
 ArduPlane/wscript                                      |  2 +-
 Tools/Hello/wscript                                    |  2 +-
 Tools/ardupilotwaf/ardupilotwaf.py                     | 10 +++++-----
 libraries/AC_PID/examples/AC_PID_test/wscript          |  2 +-
 libraries/AP_ADC/examples/AP_ADC_test/wscript          |  2 +-
 libraries/AP_AHRS/examples/AHRS_Test/wscript           |  2 +-
 libraries/AP_Airspeed/examples/Airspeed/wscript        |  2 +-
 libraries/AP_Baro/examples/BARO_generic/wscript        |  2 +-
 .../examples/AP_BattMonitor_test/wscript               |  2 +-
 libraries/AP_Common/examples/AP_Common/wscript         |  2 +-
 libraries/AP_Compass/examples/AP_Compass_test/wscript  |  2 +-
 .../examples/AP_Declination_test/wscript               |  2 +-
 libraries/AP_GPS/examples/GPS_AUTO_test/wscript        |  2 +-
 .../AP_GPS/examples/GPS_UBLOX_passthrough/wscript      |  2 +-
 libraries/AP_HAL/examples/AnalogIn/wscript             |  2 +-
 libraries/AP_HAL/examples/Printf/wscript               |  2 +-
 libraries/AP_HAL/examples/RCInput/wscript              |  2 +-
 libraries/AP_HAL/examples/RCInputToRCOutput/wscript    |  2 +-
 libraries/AP_HAL/examples/RCOutput/wscript             |  2 +-
 libraries/AP_HAL/examples/Storage/wscript              |  2 +-
 libraries/AP_HAL/examples/UART_test/wscript            |  2 +-
 libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript   |  2 +-
 libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript    |  2 +-
 libraries/AP_HAL_AVR/examples/Blink/wscript            |  2 +-
 libraries/AP_HAL_AVR/examples/Console/wscript          |  2 +-
 .../AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript     |  2 +-
 libraries/AP_HAL_AVR/examples/LCDTest/wscript          |  2 +-
 libraries/AP_HAL_AVR/examples/RCInputTest/wscript      |  2 +-
 libraries/AP_HAL_AVR/examples/RCJitterTest/wscript     |  2 +-
 .../AP_HAL_AVR/examples/RCPassthroughTest/wscript      |  2 +-
 .../AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript      |  2 +-
 libraries/AP_HAL_AVR/examples/Scheduler/wscript        |  2 +-
 libraries/AP_HAL_AVR/examples/Semaphore/wscript        |  2 +-
 libraries/AP_HAL_AVR/examples/Storage/wscript          |  2 +-
 libraries/AP_HAL_AVR/examples/UARTDriver/wscript       |  2 +-
 .../AP_HAL_AVR/examples/UtilityStringTest/wscript      |  2 +-
 .../examples/AP_Baro_BMP085_test/wscript               |  2 +-
 libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript    |  2 +-
 libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript       |  2 +-
 libraries/AP_HAL_FLYMAPLE/examples/Console/wscript     |  2 +-
 .../examples/I2CDriver_HMC5883L/wscript                |  2 +-
 libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript     |  2 +-
 .../AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript |  2 +-
 libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript   |  2 +-
 libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript   |  2 +-
 libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript   |  2 +-
 libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript     |  2 +-
 libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript  |  2 +-
 .../AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript |  2 +-
 .../AP_HAL_FLYMAPLE/examples/empty_example/wscript     |  2 +-
 libraries/AP_HAL_Linux/benchmarks/wscript              |  2 +-
 libraries/AP_HAL_Linux/examples/BusTest/wscript        |  2 +-
 libraries/AP_HAL_PX4/examples/simple/wscript           |  2 +-
 .../AP_InertialSensor/examples/INS_generic/wscript     |  2 +-
 libraries/AP_InertialSensor/examples/VibTest/wscript   |  2 +-
 libraries/AP_Math/benchmarks/wscript                   |  2 +-
 libraries/AP_Math/examples/eulers/wscript              |  2 +-
 libraries/AP_Math/examples/location/wscript            |  2 +-
 libraries/AP_Math/examples/polygon/wscript             |  2 +-
 libraries/AP_Math/examples/rotations/wscript           |  2 +-
 libraries/AP_Math/tests/wscript                        |  2 +-
 libraries/AP_Mission/examples/AP_Mission_test/wscript  |  2 +-
 .../AP_Motors/examples/AP_Motors_Time_test/wscript     |  2 +-
 libraries/AP_Motors/examples/AP_Motors_test/wscript    |  2 +-
 libraries/AP_Mount/examples/trivial_AP_Mount/wscript   |  2 +-
 libraries/AP_Notify/examples/AP_Notify_test/wscript    |  2 +-
 libraries/AP_Notify/examples/ToshibaLED_test/wscript   |  2 +-
 .../examples/AP_OpticalFlow_test/wscript               |  2 +-
 .../AP_Parachute/examples/AP_Parachute_test/wscript    |  2 +-
 libraries/AP_RangeFinder/examples/RFIND_test/wscript   |  2 +-
 libraries/AP_Scheduler/examples/Scheduler_test/wscript |  2 +-
 libraries/DataFlash/examples/DataFlash_test/wscript    |  2 +-
 libraries/Filter/examples/Derivative/wscript           |  2 +-
 libraries/Filter/examples/Filter/wscript               |  2 +-
 libraries/Filter/examples/LowPassFilter/wscript        |  2 +-
 libraries/Filter/examples/LowPassFilter2p/wscript      |  2 +-
 libraries/GCS_Console/examples/Console/wscript         |  2 +-
 libraries/GCS_MAVLink/examples/routing/wscript         |  2 +-
 libraries/PID/examples/pid/wscript                     |  2 +-
 libraries/RC_Channel/examples/RC_Channel/wscript       |  2 +-
 libraries/RC_Channel/examples/RC_UART/wscript          |  2 +-
 libraries/StorageManager/examples/StorageTest/wscript  |  2 +-
 wscript                                                |  2 +-
 86 files changed, 90 insertions(+), 90 deletions(-)

diff --git a/APMrover2/wscript b/APMrover2/wscript
index ab334221a1..a4676b3d72 100644
--- a/APMrover2/wscript
+++ b/APMrover2/wscript
@@ -6,7 +6,7 @@ def build(bld):
     bld.ap_stlib(
         name=vehicle + '_libs',
         vehicle=vehicle,
-        libraries=bld.common_vehicle_libraries() + [
+        libraries=bld.ap_common_vehicle_libraries() + [
             'APM_Control',
             'AP_Arming',
             'AP_Camera',
diff --git a/AntennaTracker/wscript b/AntennaTracker/wscript
index 090cf60235..e7cbaddffd 100644
--- a/AntennaTracker/wscript
+++ b/AntennaTracker/wscript
@@ -6,7 +6,7 @@ def build(bld):
     bld.ap_stlib(
         name=vehicle + '_libs',
         vehicle=vehicle,
-        libraries=bld.common_vehicle_libraries() + [
+        libraries=bld.ap_common_vehicle_libraries() + [
             'PID',
         ],
         use='mavlink',
diff --git a/ArduCopter/wscript b/ArduCopter/wscript
index e024c719ba..c355ac6108 100644
--- a/ArduCopter/wscript
+++ b/ArduCopter/wscript
@@ -6,7 +6,7 @@ def build(bld):
     bld.ap_stlib(
         name=vehicle + '_libs',
         vehicle=vehicle,
-        libraries=bld.common_vehicle_libraries() + [
+        libraries=bld.ap_common_vehicle_libraries() + [
             'AP_ADSB',
             'AC_AttitudeControl',
             'AC_Fence',
diff --git a/ArduPlane/wscript b/ArduPlane/wscript
index ddfc19b7f3..2421322a35 100644
--- a/ArduPlane/wscript
+++ b/ArduPlane/wscript
@@ -6,7 +6,7 @@ def build(bld):
     bld.ap_stlib(
         name=vehicle + '_libs',
         vehicle=vehicle,
-        libraries=bld.common_vehicle_libraries() + [
+        libraries=bld.ap_common_vehicle_libraries() + [
             'APM_Control',
             'APM_OBC',
             'AP_ADSB',
diff --git a/Tools/Hello/wscript b/Tools/Hello/wscript
index 36590ce02a..719ec151ba 100644
--- a/Tools/Hello/wscript
+++ b/Tools/Hello/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py
index 1d44eec5d2..d79e2a165a 100644
--- a/Tools/ardupilotwaf/ardupilotwaf.py
+++ b/Tools/ardupilotwaf/ardupilotwaf.py
@@ -65,7 +65,7 @@ IGNORED_AP_LIBRARIES = [
 ]
 
 @conf
-def get_all_libraries(bld):
+def ap_get_all_libraries(bld):
     libraries = []
     for lib_node in bld.srcnode.ant_glob('libraries/*', dir=True):
         name = lib_node.name
@@ -78,7 +78,7 @@ def get_all_libraries(bld):
     return libraries
 
 @conf
-def common_vehicle_libraries(bld):
+def ap_common_vehicle_libraries(bld):
     return COMMON_VEHICLE_DEPENDENT_LIBRARIES
 
 @conf
@@ -111,7 +111,7 @@ def ap_program(bld, blddestdir='bin',
     )
 
 @conf
-def example(bld, **kw):
+def ap_example(bld, **kw):
     kw['blddestdir'] = 'examples'
     ap_program(bld, **kw)
 
@@ -159,7 +159,7 @@ def ap_stlib(bld, **kw):
     bld.stlib(**kw)
 
 @conf
-def find_tests(bld, use=[]):
+def ap_find_tests(bld, use=[]):
     if not bld.env.HAS_GTEST:
         return
 
@@ -185,7 +185,7 @@ def find_tests(bld, use=[]):
         )
 
 @conf
-def find_benchmarks(bld, use=[]):
+def ap_find_benchmarks(bld, use=[]):
     if not bld.env.HAS_GBENCHMARK:
         return
 
diff --git a/libraries/AC_PID/examples/AC_PID_test/wscript b/libraries/AC_PID/examples/AC_PID_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AC_PID/examples/AC_PID_test/wscript
+++ b/libraries/AC_PID/examples/AC_PID_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_ADC/examples/AP_ADC_test/wscript b/libraries/AP_ADC/examples/AP_ADC_test/wscript
index 15d4b016ec..e721681dc9 100644
--- a/libraries/AP_ADC/examples/AP_ADC_test/wscript
+++ b/libraries/AP_ADC/examples/AP_ADC_test/wscript
@@ -5,6 +5,6 @@ def build(bld):
     if bld.env.BOARD in ['sitl']:
         return
 
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_AHRS/examples/AHRS_Test/wscript b/libraries/AP_AHRS/examples/AHRS_Test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_AHRS/examples/AHRS_Test/wscript
+++ b/libraries/AP_AHRS/examples/AHRS_Test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Airspeed/examples/Airspeed/wscript b/libraries/AP_Airspeed/examples/Airspeed/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Airspeed/examples/Airspeed/wscript
+++ b/libraries/AP_Airspeed/examples/Airspeed/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Baro/examples/BARO_generic/wscript b/libraries/AP_Baro/examples/BARO_generic/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Baro/examples/BARO_generic/wscript
+++ b/libraries/AP_Baro/examples/BARO_generic/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/wscript b/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/wscript
+++ b/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Common/examples/AP_Common/wscript b/libraries/AP_Common/examples/AP_Common/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Common/examples/AP_Common/wscript
+++ b/libraries/AP_Common/examples/AP_Common/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Compass/examples/AP_Compass_test/wscript b/libraries/AP_Compass/examples/AP_Compass_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Compass/examples/AP_Compass_test/wscript
+++ b/libraries/AP_Compass/examples/AP_Compass_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Declination/examples/AP_Declination_test/wscript b/libraries/AP_Declination/examples/AP_Declination_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Declination/examples/AP_Declination_test/wscript
+++ b/libraries/AP_Declination/examples/AP_Declination_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/wscript b/libraries/AP_GPS/examples/GPS_AUTO_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_GPS/examples/GPS_AUTO_test/wscript
+++ b/libraries/AP_GPS/examples/GPS_AUTO_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_GPS/examples/GPS_UBLOX_passthrough/wscript b/libraries/AP_GPS/examples/GPS_UBLOX_passthrough/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_GPS/examples/GPS_UBLOX_passthrough/wscript
+++ b/libraries/AP_GPS/examples/GPS_UBLOX_passthrough/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/AnalogIn/wscript b/libraries/AP_HAL/examples/AnalogIn/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL/examples/AnalogIn/wscript
+++ b/libraries/AP_HAL/examples/AnalogIn/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/Printf/wscript b/libraries/AP_HAL/examples/Printf/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL/examples/Printf/wscript
+++ b/libraries/AP_HAL/examples/Printf/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/RCInput/wscript b/libraries/AP_HAL/examples/RCInput/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL/examples/RCInput/wscript
+++ b/libraries/AP_HAL/examples/RCInput/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/RCInputToRCOutput/wscript b/libraries/AP_HAL/examples/RCInputToRCOutput/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL/examples/RCInputToRCOutput/wscript
+++ b/libraries/AP_HAL/examples/RCInputToRCOutput/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/RCOutput/wscript b/libraries/AP_HAL/examples/RCOutput/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL/examples/RCOutput/wscript
+++ b/libraries/AP_HAL/examples/RCOutput/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/Storage/wscript b/libraries/AP_HAL/examples/Storage/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL/examples/Storage/wscript
+++ b/libraries/AP_HAL/examples/Storage/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL/examples/UART_test/wscript b/libraries/AP_HAL/examples/UART_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL/examples/UART_test/wscript
+++ b/libraries/AP_HAL/examples/UART_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript b/libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript
+++ b/libraries/AP_HAL_AVR/examples/ArduCopterLibs/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript b/libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript
+++ b/libraries/AP_HAL_AVR/examples/ArduPlaneLibs/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/Blink/wscript b/libraries/AP_HAL_AVR/examples/Blink/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/Blink/wscript
+++ b/libraries/AP_HAL_AVR/examples/Blink/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/Console/wscript b/libraries/AP_HAL_AVR/examples/Console/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/Console/wscript
+++ b/libraries/AP_HAL_AVR/examples/Console/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript b/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript
+++ b/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/LCDTest/wscript b/libraries/AP_HAL_AVR/examples/LCDTest/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/LCDTest/wscript
+++ b/libraries/AP_HAL_AVR/examples/LCDTest/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/RCInputTest/wscript b/libraries/AP_HAL_AVR/examples/RCInputTest/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/RCInputTest/wscript
+++ b/libraries/AP_HAL_AVR/examples/RCInputTest/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/RCJitterTest/wscript b/libraries/AP_HAL_AVR/examples/RCJitterTest/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/RCJitterTest/wscript
+++ b/libraries/AP_HAL_AVR/examples/RCJitterTest/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/RCPassthroughTest/wscript b/libraries/AP_HAL_AVR/examples/RCPassthroughTest/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/RCPassthroughTest/wscript
+++ b/libraries/AP_HAL_AVR/examples/RCPassthroughTest/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript b/libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript
+++ b/libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/Scheduler/wscript b/libraries/AP_HAL_AVR/examples/Scheduler/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/Scheduler/wscript
+++ b/libraries/AP_HAL_AVR/examples/Scheduler/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/Semaphore/wscript b/libraries/AP_HAL_AVR/examples/Semaphore/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/Semaphore/wscript
+++ b/libraries/AP_HAL_AVR/examples/Semaphore/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/Storage/wscript b/libraries/AP_HAL_AVR/examples/Storage/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/Storage/wscript
+++ b/libraries/AP_HAL_AVR/examples/Storage/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/UARTDriver/wscript b/libraries/AP_HAL_AVR/examples/UARTDriver/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/UARTDriver/wscript
+++ b/libraries/AP_HAL_AVR/examples/UARTDriver/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_AVR/examples/UtilityStringTest/wscript b/libraries/AP_HAL_AVR/examples/UtilityStringTest/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_AVR/examples/UtilityStringTest/wscript
+++ b/libraries/AP_HAL_AVR/examples/UtilityStringTest/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/wscript b/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript b/libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/AnalogIn/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript b/libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/Blink/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Console/wscript b/libraries/AP_HAL_FLYMAPLE/examples/Console/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/Console/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/Console/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/wscript b/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/I2CDriver_HMC5883L/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript b/libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/RCInput/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript b/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript b/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/SPIDriver/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript b/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/Scheduler/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript b/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/Semaphore/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript b/libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/Storage/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript b/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/UARTDriver/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript b/libraries/AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/UtilityStringTest/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/empty_example/wscript b/libraries/AP_HAL_FLYMAPLE/examples/empty_example/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/empty_example/wscript
+++ b/libraries/AP_HAL_FLYMAPLE/examples/empty_example/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_Linux/benchmarks/wscript b/libraries/AP_HAL_Linux/benchmarks/wscript
index edffe3938d..32f8625c15 100644
--- a/libraries/AP_HAL_Linux/benchmarks/wscript
+++ b/libraries/AP_HAL_Linux/benchmarks/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.find_benchmarks(
+    bld.ap_find_benchmarks(
         use='ap',
     )
diff --git a/libraries/AP_HAL_Linux/examples/BusTest/wscript b/libraries/AP_HAL_Linux/examples/BusTest/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_Linux/examples/BusTest/wscript
+++ b/libraries/AP_HAL_Linux/examples/BusTest/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_HAL_PX4/examples/simple/wscript b/libraries/AP_HAL_PX4/examples/simple/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_HAL_PX4/examples/simple/wscript
+++ b/libraries/AP_HAL_PX4/examples/simple/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_InertialSensor/examples/INS_generic/wscript b/libraries/AP_InertialSensor/examples/INS_generic/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_InertialSensor/examples/INS_generic/wscript
+++ b/libraries/AP_InertialSensor/examples/INS_generic/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_InertialSensor/examples/VibTest/wscript b/libraries/AP_InertialSensor/examples/VibTest/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_InertialSensor/examples/VibTest/wscript
+++ b/libraries/AP_InertialSensor/examples/VibTest/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Math/benchmarks/wscript b/libraries/AP_Math/benchmarks/wscript
index edffe3938d..32f8625c15 100644
--- a/libraries/AP_Math/benchmarks/wscript
+++ b/libraries/AP_Math/benchmarks/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.find_benchmarks(
+    bld.ap_find_benchmarks(
         use='ap',
     )
diff --git a/libraries/AP_Math/examples/eulers/wscript b/libraries/AP_Math/examples/eulers/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Math/examples/eulers/wscript
+++ b/libraries/AP_Math/examples/eulers/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Math/examples/location/wscript b/libraries/AP_Math/examples/location/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Math/examples/location/wscript
+++ b/libraries/AP_Math/examples/location/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Math/examples/polygon/wscript b/libraries/AP_Math/examples/polygon/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Math/examples/polygon/wscript
+++ b/libraries/AP_Math/examples/polygon/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Math/examples/rotations/wscript b/libraries/AP_Math/examples/rotations/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Math/examples/rotations/wscript
+++ b/libraries/AP_Math/examples/rotations/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Math/tests/wscript b/libraries/AP_Math/tests/wscript
index 17479ab441..cd3e5e3ce7 100644
--- a/libraries/AP_Math/tests/wscript
+++ b/libraries/AP_Math/tests/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.find_tests(
+    bld.ap_find_tests(
         use='ap',
     )
diff --git a/libraries/AP_Mission/examples/AP_Mission_test/wscript b/libraries/AP_Mission/examples/AP_Mission_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Mission/examples/AP_Mission_test/wscript
+++ b/libraries/AP_Mission/examples/AP_Mission_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Motors/examples/AP_Motors_Time_test/wscript b/libraries/AP_Motors/examples/AP_Motors_Time_test/wscript
index 20cab3129f..cce0a8aab5 100644
--- a/libraries/AP_Motors/examples/AP_Motors_Time_test/wscript
+++ b/libraries/AP_Motors/examples/AP_Motors_Time_test/wscript
@@ -5,6 +5,6 @@ def build(bld):
     # TODO: Test code doesn't build. Fix or delete the test.
     return
 
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Motors/examples/AP_Motors_test/wscript b/libraries/AP_Motors/examples/AP_Motors_test/wscript
index 20cab3129f..cce0a8aab5 100644
--- a/libraries/AP_Motors/examples/AP_Motors_test/wscript
+++ b/libraries/AP_Motors/examples/AP_Motors_test/wscript
@@ -5,6 +5,6 @@ def build(bld):
     # TODO: Test code doesn't build. Fix or delete the test.
     return
 
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Mount/examples/trivial_AP_Mount/wscript b/libraries/AP_Mount/examples/trivial_AP_Mount/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Mount/examples/trivial_AP_Mount/wscript
+++ b/libraries/AP_Mount/examples/trivial_AP_Mount/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Notify/examples/AP_Notify_test/wscript b/libraries/AP_Notify/examples/AP_Notify_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Notify/examples/AP_Notify_test/wscript
+++ b/libraries/AP_Notify/examples/AP_Notify_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Notify/examples/ToshibaLED_test/wscript b/libraries/AP_Notify/examples/ToshibaLED_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Notify/examples/ToshibaLED_test/wscript
+++ b/libraries/AP_Notify/examples/ToshibaLED_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/wscript b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/wscript
+++ b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Parachute/examples/AP_Parachute_test/wscript b/libraries/AP_Parachute/examples/AP_Parachute_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Parachute/examples/AP_Parachute_test/wscript
+++ b/libraries/AP_Parachute/examples/AP_Parachute_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_RangeFinder/examples/RFIND_test/wscript b/libraries/AP_RangeFinder/examples/RFIND_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_RangeFinder/examples/RFIND_test/wscript
+++ b/libraries/AP_RangeFinder/examples/RFIND_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/AP_Scheduler/examples/Scheduler_test/wscript b/libraries/AP_Scheduler/examples/Scheduler_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/AP_Scheduler/examples/Scheduler_test/wscript
+++ b/libraries/AP_Scheduler/examples/Scheduler_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/DataFlash/examples/DataFlash_test/wscript b/libraries/DataFlash/examples/DataFlash_test/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/DataFlash/examples/DataFlash_test/wscript
+++ b/libraries/DataFlash/examples/DataFlash_test/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/Filter/examples/Derivative/wscript b/libraries/Filter/examples/Derivative/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/Filter/examples/Derivative/wscript
+++ b/libraries/Filter/examples/Derivative/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/Filter/examples/Filter/wscript b/libraries/Filter/examples/Filter/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/Filter/examples/Filter/wscript
+++ b/libraries/Filter/examples/Filter/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/Filter/examples/LowPassFilter/wscript b/libraries/Filter/examples/LowPassFilter/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/Filter/examples/LowPassFilter/wscript
+++ b/libraries/Filter/examples/LowPassFilter/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/Filter/examples/LowPassFilter2p/wscript b/libraries/Filter/examples/LowPassFilter2p/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/Filter/examples/LowPassFilter2p/wscript
+++ b/libraries/Filter/examples/LowPassFilter2p/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/GCS_Console/examples/Console/wscript b/libraries/GCS_Console/examples/Console/wscript
index 20cab3129f..cce0a8aab5 100644
--- a/libraries/GCS_Console/examples/Console/wscript
+++ b/libraries/GCS_Console/examples/Console/wscript
@@ -5,6 +5,6 @@ def build(bld):
     # TODO: Test code doesn't build. Fix or delete the test.
     return
 
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/GCS_MAVLink/examples/routing/wscript b/libraries/GCS_MAVLink/examples/routing/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/GCS_MAVLink/examples/routing/wscript
+++ b/libraries/GCS_MAVLink/examples/routing/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/PID/examples/pid/wscript b/libraries/PID/examples/pid/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/PID/examples/pid/wscript
+++ b/libraries/PID/examples/pid/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/RC_Channel/examples/RC_Channel/wscript b/libraries/RC_Channel/examples/RC_Channel/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/RC_Channel/examples/RC_Channel/wscript
+++ b/libraries/RC_Channel/examples/RC_Channel/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/RC_Channel/examples/RC_UART/wscript b/libraries/RC_Channel/examples/RC_UART/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/RC_Channel/examples/RC_UART/wscript
+++ b/libraries/RC_Channel/examples/RC_UART/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/libraries/StorageManager/examples/StorageTest/wscript b/libraries/StorageManager/examples/StorageTest/wscript
index 36590ce02a..719ec151ba 100644
--- a/libraries/StorageManager/examples/StorageTest/wscript
+++ b/libraries/StorageManager/examples/StorageTest/wscript
@@ -2,6 +2,6 @@
 # encoding: utf-8
 
 def build(bld):
-    bld.example(
+    bld.ap_example(
         use='ap',
     )
diff --git a/wscript b/wscript
index f8f3105fe2..785d34befe 100644
--- a/wscript
+++ b/wscript
@@ -141,7 +141,7 @@ def build(bld):
     bld.ap_stlib(
         name='ap',
         vehicle='UNKNOWN',
-        libraries=bld.get_all_libraries(),
+        libraries=bld.ap_get_all_libraries(),
         use='mavlink',
     )
     # TODO: Currently each vehicle also generate its own copy of the

From 53f22f4982b4da2a561e609a081217c62d562eb7 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Fri, 22 Jan 2016 20:21:09 +0000
Subject: [PATCH 11/29] waf: fix legacy defines

The following fixes where applied:
 - Value for APM_BUILD_DIRECTORY must be prefixed with APM_BUILD_
 - Renamed parameter name to sketchname, so we differentiate the real program
   name from the legacy sketch name
 - Use directory name instead of program name as argument for
   _get_legacy_defines()
---
 Tools/ardupilotwaf/ardupilotwaf.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py
index d79e2a165a..cd7b5a993e 100644
--- a/Tools/ardupilotwaf/ardupilotwaf.py
+++ b/Tools/ardupilotwaf/ardupilotwaf.py
@@ -51,11 +51,11 @@ COMMON_VEHICLE_DEPENDENT_LIBRARIES = [
     'StorageManager',
 ]
 
-def _get_legacy_defines(name):
+def _get_legacy_defines(sketch_name):
     return [
-        'APM_BUILD_DIRECTORY=' + name,
-        'SKETCH="' + name + '"',
-        'SKETCHNAME="' + name + '"',
+        'APM_BUILD_DIRECTORY=APM_BUILD_' + sketch_name,
+        'SKETCH="' + sketch_name + '"',
+        'SKETCHNAME="' + sketch_name + '"',
     ]
 
 IGNORED_AP_LIBRARIES = [
@@ -97,7 +97,7 @@ def ap_program(bld, blddestdir='bin',
         program_name = bld.path.name
 
     if use_legacy_defines:
-        kw['defines'].extend(_get_legacy_defines(program_name))
+        kw['defines'].extend(_get_legacy_defines(bld.path.name))
 
     kw['features'] = common_features(bld) + kw.get('features', [])
 

From 82322144ee7fd32ca0a815102c07c7b0aae96cd4 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Fri, 22 Jan 2016 13:23:39 -0800
Subject: [PATCH 12/29] Copter: remove unnecessary header file

---
 ArduCopter/Copter.h          |  3 ---
 ArduCopter/config_channels.h | 22 ----------------------
 2 files changed, 25 deletions(-)
 delete mode 100644 ArduCopter/config_channels.h

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 2d47452d68..8a278e99cf 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -108,12 +108,9 @@
 #include <AC_InputManager/AC_InputManager.h>        // Pilot input handling library
 #include <AC_InputManager/AC_InputManager_Heli.h>   // Heli specific pilot input handling library
 
-
-// AP_HAL to Arduino compatibility layer
 // Configuration
 #include "defines.h"
 #include "config.h"
-#include "config_channels.h"
 
 // Local modules
 #include "Parameters.h"
diff --git a/ArduCopter/config_channels.h b/ArduCopter/config_channels.h
deleted file mode 100644
index 35536d693c..0000000000
--- a/ArduCopter/config_channels.h
+++ /dev/null
@@ -1,22 +0,0 @@
-
-#ifndef __ARDUCOPTER_CONFIG_MOTORS_H__
-#define __ARDUCOPTER_CONFIG_MOTORS_H__
-
-//////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////
-//
-// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
-//
-//  DO NOT EDIT this file to adjust your configuration.  Create your own
-//  APM_Config.h and use APM_Config.h.example as a reference.
-//
-// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
-///
-//////////////////////////////////////////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////
-//
-
-#include "config.h"     // Parent Config File
-#include "APM_Config.h" // User Overrides
-
-#endif // __ARDUCOPTER_CONFIG_MOTORS_H__

From 081beacb8d49a854f73c3f0c278e904e5bb70316 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 23 Jan 2016 10:10:17 +0900
Subject: [PATCH 13/29] AP_InertialSensor: replace sqrt with safe_sqrt to
 resolve compiler warning

Also add suppressing comment for missing break at end of switch
---
 libraries/AP_InertialSensor/AP_InertialSensor.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index f4d6238a49..e7c3d2df2f 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -1334,7 +1334,7 @@ void AP_InertialSensor::_acal_save_calibrations()
                 // determine trim from aligned sample vs misaligned sample
                 Vector3f cross = (misaligned_sample%aligned_sample);
                 float dot = (misaligned_sample*aligned_sample);
-                Quaternion q(sqrt(sq(misaligned_sample.length())*sq(aligned_sample.length()))+dot, cross.x, cross.y, cross.z);
+                Quaternion q(safe_sqrt(sq(misaligned_sample.length())*sq(aligned_sample.length()))+dot, cross.x, cross.y, cross.z);
                 q.normalize();
                 _trim_roll = q.get_euler_roll();
                 _trim_pitch = q.get_euler_pitch();
@@ -1343,7 +1343,7 @@ void AP_InertialSensor::_acal_save_calibrations()
             break;
         default:
             _new_trim = false;
-            //noop
+            /* no break */
     }
 
     if (fabsf(_trim_roll) > radians(10) || 

From 5f610fdcba72b5073653a47f6f3af67b6219eee2 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 11 Jan 2016 13:29:06 -0800
Subject: [PATCH 14/29] Copter: support SET_POSITION_TARGET with WGS84
 altitudes

---
 ArduCopter/GCS_Mavlink.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index fe56953ea9..d7d1103f99 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -1717,7 +1717,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     break;
                 case MAV_FRAME_GLOBAL_INT:
                 default:
-                    loc.flags.relative_alt = false;
+                    // Copter does not support navigation to absolute altitudes. This convert the WGS84 altitude
+                    // to a home-relative altitude before passing it to the navigation controller
+                    loc.alt -= copter.ahrs.get_home().alt;
+                    loc.flags.relative_alt = true;
                     loc.flags.terrain_alt = false;
                     break;
             }

From 49ad2d26c6ee75d7725093662421d072fb366290 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 4 Jan 2016 14:39:15 -0800
Subject: [PATCH 15/29] AP_AccelCal: make client list static

---
 libraries/AP_AccelCal/AP_AccelCal.cpp |  8 ++++----
 libraries/AP_AccelCal/AP_AccelCal.h   | 12 ++++++------
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_AccelCal/AP_AccelCal.cpp b/libraries/AP_AccelCal/AP_AccelCal.cpp
index ebab633157..f75d6e94c9 100644
--- a/libraries/AP_AccelCal/AP_AccelCal.cpp
+++ b/libraries/AP_AccelCal/AP_AccelCal.cpp
@@ -22,6 +22,9 @@ const extern AP_HAL::HAL& hal;
 static bool _start_collect_sample;
 static void _snoop(const mavlink_message_t* msg);
 
+uint8_t AP_AccelCal::_num_clients = 0;
+AP_AccelCal_Client* AP_AccelCal::_clients[AP_ACCELCAL_MAX_NUM_CLIENTS] {};
+
 void AP_AccelCal::update()
 {
     if (!get_calibrator(0)) {
@@ -245,13 +248,10 @@ void AP_AccelCal::collect_sample()
 }
 
 void AP_AccelCal::register_client(AP_AccelCal_Client* client) {
-    if (client == NULL || _num_clients == AP_ACCELCAL_MAX_NUM_CLIENTS) {
+    if (client == NULL || _num_clients >= AP_ACCELCAL_MAX_NUM_CLIENTS) {
         return;
     }
 
-    if (_started) {
-        fail();
-    }
 
     for(uint8_t i=0; i<_num_clients; i++) {
         if(_clients[i] == client) {
diff --git a/libraries/AP_AccelCal/AP_AccelCal.h b/libraries/AP_AccelCal/AP_AccelCal.h
index 6ace5761e1..192ec52441 100644
--- a/libraries/AP_AccelCal/AP_AccelCal.h
+++ b/libraries/AP_AccelCal/AP_AccelCal.h
@@ -11,7 +11,6 @@ class AP_AccelCal_Client;
 class AP_AccelCal {
 public:
     AP_AccelCal():
-    _num_clients(0),
     _started(false),
     _saving(false)
     { update_status(); }
@@ -25,18 +24,19 @@ public:
     // Run an iteration of all registered calibrations
     void update();
 
-    // interface to the clients for registration
-    void register_client(AP_AccelCal_Client* client);
-
     // get the status of the calibrator server as a whole
     accel_cal_status_t get_status() { return _status; }
 
+    // interface to the clients for registration
+    static void register_client(AP_AccelCal_Client* client);
+
 private:
     GCS_MAVLINK *_gcs;
     uint8_t _step;
     accel_cal_status_t _status;
-    uint8_t _num_clients;
-    AP_AccelCal_Client* _clients[AP_ACCELCAL_MAX_NUM_CLIENTS];
+
+    static uint8_t _num_clients;
+    static AP_AccelCal_Client* _clients[AP_ACCELCAL_MAX_NUM_CLIENTS];
 
     // called on calibration success
     void success();

From 4c2e6af6eee1122a7ac9a309e8f6088db1a87fbc Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 4 Jan 2016 14:42:22 -0800
Subject: [PATCH 16/29] AP_InertialSensor: statically register with AP_AccelCal

---
 libraries/AP_InertialSensor/AP_InertialSensor.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index e7c3d2df2f..fccb056792 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -368,6 +368,8 @@ AP_InertialSensor::AP_InertialSensor() :
     }
     memset(_delta_velocity_valid,0,sizeof(_delta_velocity_valid));
     memset(_delta_angle_valid,0,sizeof(_delta_angle_valid));
+
+    AP_AccelCal::register_client(this);
 }
 
 /*
@@ -1279,8 +1281,6 @@ void AP_InertialSensor::acal_init()
     if (_accel_calibrator == NULL) {
         _accel_calibrator = new AccelCalibrator[INS_MAX_INSTANCES];
     }
-
-    _acal->register_client(this);
 }
 
 // update accel calibrator

From cbf23090238e3d6ca042bbefc94a8fad3c0e16fc Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 4 Jan 2016 15:57:17 -0800
Subject: [PATCH 17/29] AP_AHRS: add get_mag_field_NED and
 get_mag_field_correction

---
 libraries/AP_AHRS/AP_AHRS.h          | 10 ++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 46 ++++++++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  6 ++++
 3 files changed, 62 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 0d5ad7dcc6..0167403ec6 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -272,6 +272,16 @@ public:
         return false;
     }
 
+    // returns the expected NED magnetic field
+    virtual bool get_expected_mag_field_NED(Vector3f &ret) const {
+        return false;
+    }
+
+    // returns the estimated magnetic field offsets in body frame
+    virtual bool get_mag_field_correction(Vector3f &ret) const {
+        return false;
+    }
+
     // return a position relative to home in meters, North/East/Down
     // order. This will only be accurate if have_inertial_nav() is
     // true
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index bbbf6a9b64..c8366411ab 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -515,6 +515,52 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
     }
 }
 
+// returns the expected NED magnetic field
+bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const
+{
+    switch (active_EKF_type()) {
+        case EKF_TYPE_NONE:
+            return false;
+
+        case EKF_TYPE1:
+        default:
+            EKF1.getMagNED(vec);
+            return true;
+
+        case EKF_TYPE2:
+            EKF2.getMagNED(-1,vec);
+            return true;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        case EKF_TYPE_SITL:
+            return false;
+#endif
+    }
+}
+
+// returns the estimated magnetic field offsets in body frame
+bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const
+{
+    switch (active_EKF_type()) {
+        case EKF_TYPE_NONE:
+            return false;
+
+        case EKF_TYPE1:
+        default:
+            EKF1.getMagXYZ(vec);
+            return true;
+
+        case EKF_TYPE2:
+            EKF2.getMagXYZ(-1,vec);
+            return true;
+
+            #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        case EKF_TYPE_SITL:
+            return false;
+            #endif
+    }
+}
+
 // Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
 // This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for.
 bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity)
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index b4ec5ef72b..3740bf1c2f 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -197,6 +197,12 @@ public:
     // boolean false is returned if variances are not available
     bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
 
+    // returns the expected NED magnetic field
+    bool get_mag_field_NED(Vector3f& ret) const;
+
+    // returns the estimated magnetic field offsets in body frame
+    bool get_mag_field_correction(Vector3f &ret) const;
+
     void setTakeoffExpected(bool val);
     void setTouchdownExpected(bool val);
 

From 02d8b28fa3d0024c2093ab460943fc6ac4b6428c Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 4 Jan 2016 14:49:11 -0800
Subject: [PATCH 18/29] DataFlash: add GMB1, GMB2, GMB3

---
 libraries/DataFlash/LogStructure.h | 50 +++++++++++++++++++++++++++++-
 1 file changed, 49 insertions(+), 1 deletion(-)

diff --git a/libraries/DataFlash/LogStructure.h b/libraries/DataFlash/LogStructure.h
index 8859152da5..6fe1e6dc81 100644
--- a/libraries/DataFlash/LogStructure.h
+++ b/libraries/DataFlash/LogStructure.h
@@ -99,6 +99,44 @@ struct PACKED log_Vibe {
     uint32_t clipping_0, clipping_1, clipping_2;
 };
 
+struct PACKED log_Gimbal1 {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    float delta_time;
+    float delta_angles_x;
+    float delta_angles_y;
+    float delta_angles_z;
+    float delta_velocity_x;
+    float delta_velocity_y;
+    float delta_velocity_z;
+    float joint_angles_x;
+    float joint_angles_y;
+    float joint_angles_z;
+};
+
+struct PACKED log_Gimbal2 {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    uint8_t  est_sta;
+    float est_x;
+    float est_y;
+    float est_z;
+    float rate_x;
+    float rate_y;
+    float rate_z;
+    float target_x;
+    float target_y;
+    float target_z;
+};
+
+struct PACKED log_Gimbal3 {
+    LOG_PACKET_HEADER;
+    uint32_t time_ms;
+    int16_t rl_torque_cmd;
+    int16_t el_torque_cmd;
+    int16_t az_torque_cmd;
+};
+
 struct PACKED log_RCIN {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -814,7 +852,13 @@ Format characters in the format string for binary log messages
     { LOG_ORGN_MSG, sizeof(log_ORGN), \
       "ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt" }, \
     { LOG_RPM_MSG, sizeof(log_RPM), \
-      "RPM",  "Qff", "TimeUS,rpm1,rpm2" }
+      "RPM",  "Qff", "TimeUS,rpm1,rpm2" }, \
+    { LOG_GIMBAL1_MSG, sizeof(log_Gimbal1), \
+      "GMB1", "Iffffffffff", "TimeMS,dt,dax,day,daz,dvx,dvy,dvz,jx,jy,jz" }, \
+    { LOG_GIMBAL2_MSG, sizeof(log_Gimbal2), \
+      "GMB2", "IBfffffffff", "TimeMS,es,ex,ey,ez,rx,ry,rz,tx,ty,tz" }, \
+    { LOG_GIMBAL3_MSG, sizeof(log_Gimbal3), \
+      "GMB3", "Ihhh", "TimeMS,rl_torque_cmd,el_torque_cmd,az_torque_cmd" }
 
 // #if SBP_HW_LOGGING
 #define LOG_SBP_STRUCTURES \
@@ -923,6 +967,10 @@ enum LogMessages {
     LOG_MSG_SBPRAW2,
     LOG_MSG_SBPRAWx,
 
+    LOG_GIMBAL1_MSG,
+    LOG_GIMBAL2_MSG,
+    LOG_GIMBAL3_MSG,
+
 // message types 211 to 220 reversed for autotune use
 
 };

From eabede692e006bed1f79f39507ca8bf872346f48 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Wed, 30 Dec 2015 16:00:28 -0800
Subject: [PATCH 19/29] AP_SmallEKF: move to AP_Mount/SoloGimbalEKF and merge
 solo version

---
 .../SoloGimbalEKF.cpp}                        | 196 +++++++++++++-----
 .../SoloGimbalEKF.h}                          |  26 ++-
 2 files changed, 157 insertions(+), 65 deletions(-)
 rename libraries/{AP_NavEKF/AP_SmallEKF.cpp => AP_Mount/SoloGimbalEKF.cpp} (85%)
 rename libraries/{AP_NavEKF/AP_SmallEKF.h => AP_Mount/SoloGimbalEKF.h} (92%)

diff --git a/libraries/AP_NavEKF/AP_SmallEKF.cpp b/libraries/AP_Mount/SoloGimbalEKF.cpp
similarity index 85%
rename from libraries/AP_NavEKF/AP_SmallEKF.cpp
rename to libraries/AP_Mount/SoloGimbalEKF.cpp
index d9e85b63af..c267a98b12 100644
--- a/libraries/AP_NavEKF/AP_SmallEKF.cpp
+++ b/libraries/AP_Mount/SoloGimbalEKF.cpp
@@ -12,8 +12,7 @@
 #pragma GCC optimize("O3")
 #endif
 
-#include "AP_SmallEKF.h"
-#include <AP_AHRS/AP_AHRS.h>
+#include "SoloGimbalEKF.h"
 #include <AP_Param/AP_Param.h>
 #include <AP_Vehicle/AP_Vehicle.h>
 
@@ -23,47 +22,95 @@ extern const AP_HAL::HAL& hal;
 
 
 // Define tuning parameters
-const AP_Param::GroupInfo SmallEKF::var_info[] = {
+const AP_Param::GroupInfo SoloGimbalEKF::var_info[] = {
     AP_GROUPEND
 };
 
+// Hash define constants
+#define GYRO_BIAS_LIMIT 0.349066f // maximum allowed gyro bias (rad/sec)
+
 // constructor
-SmallEKF::SmallEKF(const AP_AHRS_NavEKF &ahrs) :
+SoloGimbalEKF::SoloGimbalEKF(const AP_AHRS_NavEKF &ahrs) :
     _ahrs(ahrs),
-    _main_ekf(ahrs.get_NavEKF_const()),
     states(),
-    state(*reinterpret_cast<struct state_elements *>(&states)),
-    gSense{},
-    Cov{},
-    TiltCorrection(0),
-    StartTime_ms(0),
-    FiltInit(false),
-    lastMagUpdate(0),
-    dtIMU(0)
+    state(*reinterpret_cast<struct state_elements *>(&states))
 {
     AP_Param::setup_object_defaults(this, var_info);
+    reset();
+}
+
+
+// complete reset
+void SoloGimbalEKF::reset()
+{
+    memset(&states,0,sizeof(states));
+    memset(&gSense,0,sizeof(gSense));
+    memset(&Cov,0,sizeof(Cov));
+    TiltCorrection = 0;
+    StartTime_ms = 0;
+    FiltInit = false;
+    lastMagUpdate = 0;
+    dtIMU = 0;
+    innovationIncrement = 0;
+    lastInnovation = 0;
 }
 
 // run a 9-state EKF used to calculate orientation
-void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
+void SoloGimbalEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
 {
     imuSampleTime_ms = AP_HAL::millis();
     dtIMU = delta_time;
 
     // initialise variables and constants
     if (!FiltInit) {
-        StartTime_ms =  imuSampleTime_ms;
+        // Note: the start time is initialised to 0 in the constructor
+        if (StartTime_ms == 0) {
+            StartTime_ms = imuSampleTime_ms;
+        }
+
+        // Set data to pre-initialsation defaults
+        FiltInit = false;
         newDataMag = false;
         YawAligned = false;
+        memset(&state, 0, sizeof(state));
         state.quat[0] = 1.0f;
+
+        bool main_ekf_healthy = false;
+        nav_filter_status main_ekf_status;
+
+        if (_ahrs.get_filter_status(main_ekf_status)) {
+            if (main_ekf_status.flags.attitude) {
+                main_ekf_healthy = true;
+            }
+        }
+
+        // Wait for gimbal to stabilise to body fixed position for a few seconds before starting small EKF
+        // Also wait for navigation EKF to be healthy beasue we are using the velocity output data
+        // This prevents jerky gimbal motion from degrading the EKF initial state estimates
+        if (imuSampleTime_ms - StartTime_ms < 5000 || !main_ekf_healthy) {
+            return;
+        }
+
+        Quaternion ned_to_vehicle_quat;
+        ned_to_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
+
+        Quaternion vehicle_to_gimbal_quat;
+        vehicle_to_gimbal_quat.from_vector312(joint_angles.x,joint_angles.y,joint_angles.z);
+
+        // calculate initial orientation
+        state.quat = ned_to_vehicle_quat * vehicle_to_gimbal_quat;
+
         const float Sigma_velNED = 0.5f; // 1 sigma uncertainty in horizontal velocity components
-        const float Sigma_dAngBias  = 0.01745f*dtIMU; // 1 Sigma uncertainty in delta angle bias
-        const float Sigma_angErr = 1.0f; // 1 Sigma uncertainty in angular misalignment (rad)
+        const float Sigma_dAngBias = 0.002f*dtIMU; // 1 Sigma uncertainty in delta angle bias (rad)
+        const float Sigma_angErr = 0.1f; // 1 Sigma uncertainty in angular misalignment (rad)
         for (uint8_t i=0; i <= 2; i++) Cov[i][i] = sq(Sigma_angErr);
         for (uint8_t i=3; i <= 5; i++) Cov[i][i] = sq(Sigma_velNED);
         for (uint8_t i=6; i <= 8; i++) Cov[i][i] = sq(Sigma_dAngBias);
         FiltInit = true;
-        hal.console->printf("\nSmallEKF Alignment Started\n");
+        hal.console->printf("\nSoloGimbalEKF Alignment Started\n");
+
+        // Don't run the filter in this timestep becasue we have already used the delta velocity data to set an initial orientation
+        return;
     }
 
     // We are using IMU data and joint angles from the gimbal
@@ -85,17 +132,17 @@ void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vect
     // predict the covariance
     predictCovariance();
 
-    // fuse SmallEKF velocity data
-    fuseVelocity(YawAligned);
+    // fuse SoloGimbalEKF velocity data
+    fuseVelocity();
 
     
     // Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold
     // Force it to align if too much time has lapsed
-    if (((((imuSampleTime_ms - StartTime_ms) > 25000 && TiltCorrection < 1e-4f) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
+    if (((((imuSampleTime_ms - StartTime_ms) > 8000 && TiltCorrection < 1e-4f) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
         //calculate the initial heading using magnetometer, estimated tilt and declination
         alignHeading();
         YawAligned = true;
-        hal.console->printf("\nSmallEKF Alignment Completed\n");
+        hal.console->printf("\nSoloGimbalEKF Alignment Completed\n");
     }
 
     // Fuse magnetometer data if  we have new measurements and an aligned heading
@@ -109,7 +156,7 @@ void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vect
 }
 
 // state prediction
-void SmallEKF::predictStates()
+void SoloGimbalEKF::predictStates()
 {
     static Vector3f gimDelAngCorrected;
     static Vector3f gimDelAngPrev;
@@ -138,14 +185,21 @@ void SmallEKF::predictStates()
 
     // sum delta velocities to get velocity
     state.velocity += delVelNav;
+
+    state.delAngBias.x = constrain_float(state.delAngBias.x, -GYRO_BIAS_LIMIT*dtIMU,GYRO_BIAS_LIMIT*dtIMU);
+    state.delAngBias.y = constrain_float(state.delAngBias.y, -GYRO_BIAS_LIMIT*dtIMU,GYRO_BIAS_LIMIT*dtIMU);
+    state.delAngBias.z = constrain_float(state.delAngBias.z, -GYRO_BIAS_LIMIT*dtIMU,GYRO_BIAS_LIMIT*dtIMU);
 }
 
 // covariance prediction using optimised algebraic toolbox expressions
 // equivalent to P = F*P*transpose(P) + G*imu_errors*transpose(G) +
 // gyro_bias_state_noise
-void SmallEKF::predictCovariance()
+void SoloGimbalEKF::predictCovariance()
 {
-    float delAngBiasVariance = sq(dtIMU*dtIMU*5E-4f);
+    float delAngBiasVariance = sq(dtIMU*5E-6f);
+    if (YawAligned && !hal.util->get_soft_armed()) {
+        delAngBiasVariance *= 4.0f;
+    }
 
     float daxNoise = sq(dtIMU*0.0087f);
     float dayNoise = sq(dtIMU*0.0087f);
@@ -540,9 +594,13 @@ void SmallEKF::predictCovariance()
 
 }
 
-// Fuse the SmallEKF velocity estimates - this enables alevel reference to be maintained during constant turns
-void SmallEKF::fuseVelocity(bool yawInit)
+// Fuse the SoloGimbalEKF velocity estimates - this enables alevel reference to be maintained during constant turns
+void SoloGimbalEKF::fuseVelocity()
 {
+    if (!_ahrs.have_inertial_nav()) {
+        return;
+    }
+
     float R_OBS = 0.25f;
     float innovation[3];
     float varInnov[3];
@@ -553,11 +611,18 @@ void SmallEKF::fuseVelocity(bool yawInit)
     for (uint8_t obsIndex=0;obsIndex<=2;obsIndex++) {
         stateIndex = 3 + obsIndex;
 
-        // Calculate the velocity measurement innovation using the SmallEKF estimate as the observation
+        // Calculate the velocity measurement innovation using the SoloGimbalEKF estimate as the observation
         // if heading isn't aligned, use zero velocity (static assumption)
-        if (yawInit) {
-            Vector3f measVelNED;
-            _main_ekf.getVelNED(measVelNED);
+        if (YawAligned) {
+            Vector3f measVelNED = Vector3f(0,0,0);
+            nav_filter_status main_ekf_status;
+
+            if (_ahrs.get_filter_status(main_ekf_status)) {
+                if (main_ekf_status.flags.horiz_vel) {
+                    _ahrs.get_velocity_NED(measVelNED);
+                }
+            }
+
             innovation[obsIndex] = state.velocity[obsIndex] - measVelNED[obsIndex];
         } else {
             innovation[obsIndex] = state.velocity[obsIndex];
@@ -599,10 +664,10 @@ void SmallEKF::fuseVelocity(bool yawInit)
 }
 
 // check for new magnetometer data and update store measurements if available
-void SmallEKF::readMagData()
+void SoloGimbalEKF::readMagData()
 {
-    if (_ahrs.get_compass() && 
-        _ahrs.get_compass()->use_for_yaw() && 
+    if (_ahrs.get_compass() &&
+        _ahrs.get_compass()->use_for_yaw() &&
         _ahrs.get_compass()->last_update_usec() != lastMagUpdate) {
         // store time of last measurement update
         lastMagUpdate = _ahrs.get_compass()->last_update_usec();
@@ -619,7 +684,7 @@ void SmallEKF::readMagData()
 }
 
 // Fuse compass measurements from autopilot
-void SmallEKF::fuseCompass()
+void SoloGimbalEKF::fuseCompass()
 {
     float q0 = state.quat[0];
     float q1 = state.quat[1];
@@ -766,11 +831,10 @@ void SmallEKF::fuseCompass()
 }
 
 // Perform an initial heading alignment using the magnetic field and assumed declination
-void SmallEKF::alignHeading()
+void SoloGimbalEKF::alignHeading()
 {
     // calculate the correction rotation vector in NED frame
-    Vector3f deltaRotNED;
-    deltaRotNED.z = -calcMagHeadingInnov();
+    Vector3f deltaRotNED = Vector3f(0,0,-calcMagHeadingInnov());
 
     // rotate into sensor frame
     Vector3f angleCorrection = Tsn.transposed()*deltaRotNED;
@@ -785,7 +849,7 @@ void SmallEKF::alignHeading()
 
 
 // Calculate magnetic heading innovation
-float SmallEKF::calcMagHeadingInnov()
+float SoloGimbalEKF::calcMagHeadingInnov()
 {
     // Define rotation from magnetometer to sensor using a 312 rotation sequence
     Matrix3f Tms;
@@ -800,18 +864,19 @@ float SmallEKF::calcMagHeadingInnov()
     Tms[2][2] = cosTheta*cosPhi;
 
     // get earth magnetic field estimate from main ekf if available to take advantage of main ekf magnetic field learning
-    Vector3f body_magfield, earth_magfield;
+    Vector3f earth_magfield = Vector3f(0,0,0);
+    _ahrs.get_mag_field_NED(earth_magfield);
+
     float declination;
-    if (_main_ekf.healthy()) {
-        _main_ekf.getMagNED(earth_magfield);
-        _main_ekf.getMagXYZ(body_magfield);
+    if (!earth_magfield.is_zero()) {
         declination = atan2f(earth_magfield.y,earth_magfield.x);
     } else {
-        body_magfield.zero();
-        earth_magfield.zero();
         declination = _ahrs.get_compass()->get_declination();
     }
 
+    Vector3f body_magfield = Vector3f(0,0,0);
+    _ahrs.get_mag_field_correction(body_magfield);
+
     // Define rotation from magnetometer to NED axes
     Matrix3f Tmn = Tsn*Tms;
 
@@ -822,17 +887,27 @@ float SmallEKF::calcMagHeadingInnov()
     float innovation = atan2f(magMeasNED.y,magMeasNED.x) - declination;
 
     // wrap the innovation so it sits on the range from +-pi
-    if (innovation > 3.1415927f) {
-        innovation = innovation - 6.2831853f;
-    } else if (innovation < -3.1415927f) {
-        innovation = innovation + 6.2831853f;
+    if (innovation > M_PI_F) {
+        innovation = innovation - 2*M_PI_F;
+    } else if (innovation < -M_PI_F) {
+        innovation = innovation + 2*M_PI_F;
     }
 
-    return innovation;
+    // Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift
+    if (innovation - lastInnovation > M_PI_F) {
+        // Angle has wrapped in the positive direction to subtract an additional 2*Pi
+        innovationIncrement -= 2*M_PI_F;
+    } else if (innovation -innovationIncrement < -M_PI_F) {
+        // Angle has wrapped in the negative direction so add an additional 2*Pi
+        innovationIncrement += 2*M_PI_F;
+    }
+    lastInnovation = innovation;
+
+    return innovation + innovationIncrement;
 }
 
 // Force symmmetry and non-negative diagonals on state covarinace matrix
-void SmallEKF::fixCovariance()
+void SoloGimbalEKF::fixCovariance()
 {
     // force symmetry
     for (uint8_t rowIndex=1; rowIndex<=8; rowIndex++) {
@@ -851,7 +926,7 @@ void SmallEKF::fixCovariance()
 }
 
 // return data for debugging EKF
-void SmallEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const
+void SoloGimbalEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const
 {
     tilt = TiltCorrection;
     velocity = state.velocity;
@@ -864,7 +939,7 @@ void SmallEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector
 }
 
 // get gyro bias data
-void SmallEKF::getGyroBias(Vector3f &gyroBias) const
+void SoloGimbalEKF::getGyroBias(Vector3f &gyroBias) const
 {
     if (dtIMU < 1.0e-6f) {
         gyroBias.zero();
@@ -873,17 +948,26 @@ void SmallEKF::getGyroBias(Vector3f &gyroBias) const
     }
 }
 
+void SoloGimbalEKF::setGyroBias(const Vector3f &gyroBias)
+{
+    if (dtIMU < 1.0e-6f) {
+        return;
+    }
+    state.delAngBias = gyroBias * dtIMU;
+}
+
+
 // get quaternion data
-void SmallEKF::getQuat(Quaternion &quat) const
+void SoloGimbalEKF::getQuat(Quaternion &quat) const
 {
     quat = state.quat;
 }
 
 // get filter status - true is aligned
-bool SmallEKF::getStatus() const
+bool SoloGimbalEKF::getStatus() const
 {
     float run_time = AP_HAL::millis() - StartTime_ms;
-    return  YawAligned && (run_time > 30000);
+    return  YawAligned && (run_time > 15000);
 }
 
 #endif // HAL_CPU_CLASS
diff --git a/libraries/AP_NavEKF/AP_SmallEKF.h b/libraries/AP_Mount/SoloGimbalEKF.h
similarity index 92%
rename from libraries/AP_NavEKF/AP_SmallEKF.h
rename to libraries/AP_Mount/SoloGimbalEKF.h
index 597b012c4b..874ea2a5ec 100644
--- a/libraries/AP_NavEKF/AP_SmallEKF.h
+++ b/libraries/AP_Mount/SoloGimbalEKF.h
@@ -18,8 +18,8 @@
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef AP_SmallEKF
-#define AP_SmallEKF
+#ifndef _SOLO_GIMBAL_EKF_
+#define _SOLO_GIMBAL_EKF_
 
 #include <AP_Math/AP_Math.h>
 #include <AP_InertialSensor/AP_InertialSensor.h>
@@ -27,13 +27,13 @@
 #include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_Compass/AP_Compass.h>
 #include <AP_Param/AP_Param.h>
-#include "AP_Nav_Common.h"
+#include <AP_NavEKF/AP_Nav_Common.h>
 #include <AP_AHRS/AP_AHRS.h>
-#include "AP_NavEKF.h"
+#include <AP_NavEKF/AP_NavEKF.h>
 
 #include <AP_Math/vectorN.h>
 
-class SmallEKF
+class SoloGimbalEKF
 {
 public:
     typedef float ftype;
@@ -78,17 +78,22 @@ public:
 #endif
 
     // Constructor
-    SmallEKF(const AP_AHRS_NavEKF &ahrs);
+    SoloGimbalEKF(const AP_AHRS_NavEKF &ahrs);
 
     // Run the EKF main loop once every time we receive sensor data
     void RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles);
 
+    void reset();
+
     // get some debug information
     void getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const;
 
     // get gyro bias data
     void getGyroBias(Vector3f &gyroBias) const;
 
+    // set gyro bias
+    void setGyroBias(const Vector3f &gyroBias);
+
     // get quaternion data
     void getQuat(Quaternion &quat) const;
 
@@ -99,7 +104,6 @@ public:
 
 private:
     const AP_AHRS_NavEKF &_ahrs;
-    const NavEKF &_main_ekf;
 
     // the states are available in two forms, either as a Vector13 or
     // broken down as individual elements. Both are equivalent (same
@@ -140,6 +144,10 @@ private:
     uint32_t imuSampleTime_ms;
     float dtIMU;
 
+    // States used for unwrapping of compass yaw error
+    float innovationIncrement;
+    float lastInnovation;
+
     // state prediction
     void predictStates();
 
@@ -147,7 +155,7 @@ private:
     void predictCovariance();
 
     // EKF velocity fusion
-    void fuseVelocity(bool yawInit);
+    void fuseVelocity();
     
     // read magnetometer data
     void readMagData();
@@ -165,4 +173,4 @@ private:
     void fixCovariance();
 };
 
-#endif // AP_SmallEKF
+#endif // _SOLO_GIMBAL_EKF_

From 5b834330cb5176f9cd1dacb85143d349529e1db4 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Wed, 30 Dec 2015 18:19:52 -0800
Subject: [PATCH 20/29] AP_Mount: merge SoloGimbal from solo master

---
 libraries/AP_Mount/AP_Gimbal.cpp              | 212 --------
 libraries/AP_Mount/AP_Gimbal.h                |  95 ----
 libraries/AP_Mount/AP_Mount.cpp               | 108 ++--
 libraries/AP_Mount/AP_Mount.h                 |  27 +-
 libraries/AP_Mount/AP_Mount_Backend.h         |   6 +
 ...nt_MAVLink.cpp => AP_Mount_SoloGimbal.cpp} |  73 ++-
 ..._Mount_MAVLink.h => AP_Mount_SoloGimbal.h} |  25 +-
 libraries/AP_Mount/SoloGimbal.cpp             | 489 ++++++++++++++++++
 libraries/AP_Mount/SoloGimbal.h               | 160 ++++++
 libraries/AP_Mount/SoloGimbal_Parameters.cpp  | 278 ++++++++++
 libraries/AP_Mount/SoloGimbal_Parameters.h    |  94 ++++
 11 files changed, 1149 insertions(+), 418 deletions(-)
 delete mode 100644 libraries/AP_Mount/AP_Gimbal.cpp
 delete mode 100644 libraries/AP_Mount/AP_Gimbal.h
 rename libraries/AP_Mount/{AP_Mount_MAVLink.cpp => AP_Mount_SoloGimbal.cpp} (53%)
 rename libraries/AP_Mount/{AP_Mount_MAVLink.h => AP_Mount_SoloGimbal.h} (69%)
 create mode 100644 libraries/AP_Mount/SoloGimbal.cpp
 create mode 100644 libraries/AP_Mount/SoloGimbal.h
 create mode 100644 libraries/AP_Mount/SoloGimbal_Parameters.cpp
 create mode 100644 libraries/AP_Mount/SoloGimbal_Parameters.h

diff --git a/libraries/AP_Mount/AP_Gimbal.cpp b/libraries/AP_Mount/AP_Gimbal.cpp
deleted file mode 100644
index 3bc29b592a..0000000000
--- a/libraries/AP_Mount/AP_Gimbal.cpp
+++ /dev/null
@@ -1,212 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-#include "AP_Gimbal.h"
-
-#if AP_AHRS_NAVEKF_AVAILABLE
-
-#include <stdio.h>
-#include <AP_Common/AP_Common.h>
-#include <GCS_MAVLink/GCS.h>
-#include <AP_NavEKF/AP_SmallEKF.h>
-#include <AP_Math/AP_Math.h>
-
-void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
-{
-    decode_feedback(msg);
-    update_state();
-    if (_ekf.getStatus() && !isCopterFlipped() && !is_zero(_gimbalParams.K_gimbalRate)){
-        send_control(chan);
-    }
-
-    Quaternion quatEst;_ekf.getQuat(quatEst);Vector3f eulerEst;quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
-    //::printf("est=%1.1f %1.1f %1.1f %d\t", eulerEst.x,eulerEst.y,eulerEst.z,_ekf.getStatus());    
-    //::printf("joint_angles=(%+1.2f %+1.2f %+1.2f)\t", _measurement.joint_angles.x,_measurement.joint_angles.y,_measurement.joint_angles.z);
-    //::printf("delta_ang=(%+1.3f %+1.3f %+1.3f)\t",_measurement.delta_angles.x,_measurement.delta_angles.y,_measurement.delta_angles.z);     
-    //::printf("delta_vel=(%+1.3f %+1.3f %+1.3f)\t",_measurement.delta_velocity.x,_measurement.delta_velocity.y,_measurement.delta_velocity.z);     
-    //::printf("rate=(%+1.3f %+1.3f %+1.3f)\t",gimbalRateDemVec.x,gimbalRateDemVec.y,gimbalRateDemVec.z);
-    //::printf("target=(%+1.3f %+1.3f %+1.3f)\t",_angle_ef_target_rad.x,_angle_ef_target_rad.y,_angle_ef_target_rad.z);
-    //::printf("\n");
-}
-
-void AP_Gimbal::decode_feedback(mavlink_message_t *msg)
-{
-    mavlink_msg_gimbal_report_decode(msg, &_report_msg);
-
-    _measurement.delta_time = _report_msg.delta_time;
-    _measurement.delta_angles.x = _report_msg.delta_angle_x;
-    _measurement.delta_angles.y = _report_msg.delta_angle_y;
-    _measurement.delta_angles.z = _report_msg.delta_angle_z;
-    _measurement.delta_velocity.x = _report_msg.delta_velocity_x,
-    _measurement.delta_velocity.y = _report_msg.delta_velocity_y;
-    _measurement.delta_velocity.z = _report_msg.delta_velocity_z;
-    _measurement.joint_angles.x = _report_msg.joint_roll;
-    _measurement.joint_angles.y = _report_msg.joint_el;
-    _measurement.joint_angles.z = _report_msg.joint_az;
-
-    //apply joint angle compensation
-    _measurement.joint_angles -= _gimbalParams.joint_angles_offsets;
-    _measurement.delta_velocity -= _gimbalParams.delta_velocity_offsets;
-    _measurement.delta_angles -= _gimbalParams.delta_angles_offsets;
-}
-
-/*
-  send a gimbal report to the GCS for display purposes
- */
-void AP_Gimbal::send_report(mavlink_channel_t chan) const
-{
-    mavlink_msg_gimbal_report_send(chan, 
-                                   0, 0, // send as broadcast
-                                   _report_msg.delta_time, 
-                                   _report_msg.delta_angle_x, 
-                                   _report_msg.delta_angle_y, 
-                                   _report_msg.delta_angle_z, 
-                                   _report_msg.delta_velocity_x, 
-                                   _report_msg.delta_velocity_y, 
-                                   _report_msg.delta_velocity_z, 
-                                   _report_msg.joint_roll, 
-                                   _report_msg.joint_el, 
-                                   _report_msg.joint_az);
-}
-
-void AP_Gimbal::update_state()
-{
-    // Run the gimbal attitude and gyro bias estimator
-    _ekf.RunEKF(_measurement.delta_time, _measurement.delta_angles, _measurement.delta_velocity, _measurement.joint_angles);
-
-    // get the gimbal quaternion estimate
-    Quaternion quatEst;
-    _ekf.getQuat(quatEst);
-
-    // Add the control rate vectors
-    gimbalRateDemVec.zero();
-    gimbalRateDemVec += getGimbalRateDemVecYaw(quatEst);
-    gimbalRateDemVec += getGimbalRateDemVecTilt(quatEst);
-    gimbalRateDemVec += getGimbalRateDemVecForward(quatEst);
-    gimbalRateDemVec += getGimbalRateDemVecGyroBias();
-}
-
-Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const Quaternion &quatEst)
-{
-        // Define rotation from vehicle to gimbal using a 312 rotation sequence
-        Matrix3f Tvg;
-        float cosPhi = cosf(_measurement.joint_angles.x);
-        float cosTheta = cosf(_measurement.joint_angles.y);
-        float sinPhi = sinf(_measurement.joint_angles.x);
-        float sinTheta = sinf(_measurement.joint_angles.y);
-        float sinPsi = sinf(_measurement.joint_angles.z);
-        float cosPsi = cosf(_measurement.joint_angles.z);
-        Tvg[0][0] = cosTheta*cosPsi-sinPsi*sinPhi*sinTheta;
-        Tvg[1][0] = -sinPsi*cosPhi;
-        Tvg[2][0] = cosPsi*sinTheta+cosTheta*sinPsi*sinPhi;
-        Tvg[0][1] = cosTheta*sinPsi+cosPsi*sinPhi*sinTheta;
-        Tvg[1][1] = cosPsi*cosPhi;
-        Tvg[2][1] = sinPsi*sinTheta-cosTheta*cosPsi*sinPhi;
-        Tvg[0][2] = -sinTheta*cosPhi;
-        Tvg[1][2] = sinPhi;
-        Tvg[2][2] = cosTheta*cosPhi;
-
-        // multiply the yaw joint angle by a gain to calculate a demanded vehicle frame relative rate vector required to keep the yaw joint centred
-        Vector3f gimbalRateDemVecYaw;
-        gimbalRateDemVecYaw.z = - _gimbalParams.K_gimbalRate * _measurement.joint_angles.z;
-
-        // Get filtered vehicle turn rate in earth frame
-        vehicleYawRateFilt = (1.0f - yawRateFiltPole * _measurement.delta_time) * vehicleYawRateFilt + yawRateFiltPole * _measurement.delta_time * _ahrs.get_yaw_rate_earth();
-        Vector3f vehicle_rate_ef(0,0,vehicleYawRateFilt);
-
-         // calculate the maximum steady state rate error corresponding to the maximum permitted yaw angle error
-        float maxRate = _gimbalParams.K_gimbalRate * yawErrorLimit;
-        float vehicle_rate_mag_ef = vehicle_rate_ef.length();
-        float excess_rate_correction = fabsf(vehicle_rate_mag_ef) - maxRate; 
-        if (vehicle_rate_mag_ef > maxRate) {
-            if (vehicle_rate_ef.z>0.0f){
-                gimbalRateDemVecYaw += _ahrs.get_rotation_body_to_ned().transposed()*Vector3f(0,0,excess_rate_correction);
-            } else {
-                gimbalRateDemVecYaw -= _ahrs.get_rotation_body_to_ned().transposed()*Vector3f(0,0,excess_rate_correction);
-            }
-        }
-
-        // rotate into gimbal frame to calculate the gimbal rate vector required to keep the yaw gimbal centred
-        gimbalRateDemVecYaw = Tvg * gimbalRateDemVecYaw;
-        return gimbalRateDemVecYaw;
-}
-
-Vector3f AP_Gimbal::getGimbalRateDemVecTilt(const Quaternion &quatEst)
-{
-        // Calculate the gimbal 321 Euler angle estimates relative to earth frame
-        Vector3f eulerEst = quatEst.to_vector312();
-
-        // Calculate a demanded quaternion using the demanded roll and pitch and estimated yaw (yaw is slaved to the vehicle)
-        Quaternion quatDem;
-        quatDem.from_vector312( _angle_ef_target_rad.x,
-                                _angle_ef_target_rad.y,
-                                eulerEst.z);
-
-        //divide the demanded quaternion by the estimated to get the error
-        Quaternion quatErr = quatDem / quatEst;
-
-        // Convert to a delta rotation using a small angle approximation
-        quatErr.normalize();
-        Vector3f deltaAngErr;
-        float scaler;
-        if (quatErr[0] >= 0.0f) {
-            scaler = 2.0f;
-        } else {
-            scaler = -2.0f;
-        }
-        deltaAngErr.x = scaler * quatErr[1];
-        deltaAngErr.y = scaler * quatErr[2];
-        deltaAngErr.z = scaler * quatErr[3];
-
-        // multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt
-        Vector3f gimbalRateDemVecTilt = deltaAngErr * _gimbalParams.K_gimbalRate;
-        return gimbalRateDemVecTilt;
-}
-
-Vector3f AP_Gimbal::getGimbalRateDemVecForward(const Quaternion &quatEst)
-{
-        // quaternion demanded at the previous time step
-        static float lastDem;
-
-        // calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation
-        float delta = _angle_ef_target_rad.y - lastDem;
-        lastDem = _angle_ef_target_rad.y;
-
-        Vector3f gimbalRateDemVecForward;
-        gimbalRateDemVecForward.y = delta / _measurement.delta_time;
-        return gimbalRateDemVecForward;
-}
-
-Vector3f AP_Gimbal::getGimbalRateDemVecGyroBias()
-{
-    Vector3f gyroBias;
-    _ekf.getGyroBias(gyroBias);
-    return gyroBias;
-}
-
-void AP_Gimbal::send_control(mavlink_channel_t chan)
-{
-    mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
-        gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z);
-}
-
-void AP_Gimbal::update_target(Vector3f newTarget)
-{
-    // Low-pass filter
-    _angle_ef_target_rad.y = _angle_ef_target_rad.y + 0.02f*(newTarget.y - _angle_ef_target_rad.y);
-    // Update tilt
-    _angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y,radians(-90),radians(0));
-}
-
-Vector3f AP_Gimbal::getGimbalEstimateEF()
-{
-    Quaternion quatEst;
-    _ekf.getQuat(quatEst);
-    return quatEst.to_vector312();
-}
-
-bool AP_Gimbal::isCopterFlipped()
-{
-    return (_ahrs.cos_roll()*_ahrs.cos_pitch() < 0.5f);
-}
-
-#endif // AP_AHRS_NAVEKF_AVAILABLE
diff --git a/libraries/AP_Mount/AP_Gimbal.h b/libraries/AP_Mount/AP_Gimbal.h
deleted file mode 100644
index aa9d362275..0000000000
--- a/libraries/AP_Mount/AP_Gimbal.h
+++ /dev/null
@@ -1,95 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-/************************************************************
-* AP_Gimbal -- library to control a 3 axis rate gimbal.     *
-*                                                           *
-* Author:  Arthur Benemann, Paul Riseborough;               *
-*                                                           *
-************************************************************/
-#ifndef __AP_GIMBAL_H__
-#define __AP_GIMBAL_H__
-
-#include <AP_HAL/AP_HAL.h>
-#include <AP_AHRS/AP_AHRS.h>
-
-#if AP_AHRS_NAVEKF_AVAILABLE
-
-#include <AP_Math/AP_Math.h>
-#include <AP_Common/AP_Common.h>
-#include <AP_GPS/AP_GPS.h>
-#include <GCS_MAVLink/GCS_MAVLink.h>
-#include "AP_Mount.h"
-#include <AP_NavEKF/AP_SmallEKF.h>
-#include <AP_NavEKF/AP_NavEKF.h>
-
-class AP_Gimbal
-{
-public:
-    //Constructor
-    AP_Gimbal(const AP_AHRS_NavEKF &ahrs, uint8_t compid, const AP_Mount::gimbal_params &gimbalParams) :
-        _ekf(ahrs),
-        _ahrs(ahrs),
-        _gimbalParams(gimbalParams),
-        vehicleYawRateFilt(0.0f),
-        yawRateFiltPole(10.0f),
-        yawErrorLimit(0.1f)
-    {
-        _compid = compid;
-        memset(&_report_msg, 0, sizeof(_report_msg));
-    }
-
-    void    update_target(Vector3f newTarget);
-    void    receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
-    void    send_report(mavlink_channel_t chan) const;
-
-    Vector3f getGimbalEstimateEF();
-
-    struct Measurament {
-        float delta_time;
-        Vector3f delta_angles;
-        Vector3f delta_velocity;
-        Vector3f joint_angles;
-    } _measurement;
-
-    SmallEKF    _ekf;                   // state of small EKF for gimbal
-    const AP_AHRS_NavEKF    &_ahrs;     //  Main EKF
-    Vector3f    gimbalRateDemVec;       // degrees/s
-    Vector3f    _angle_ef_target_rad;   // desired earth-frame roll, tilt and pan angles in radians
-
-private:
-    const AP_Mount::gimbal_params &_gimbalParams;
-
-    // filtered yaw rate from the vehicle
-    float vehicleYawRateFilt;
-
-    // circular frequency (rad/sec) constant of filter applied to forward path vehicle yaw rate
-    // this frequency must not be larger than the update rate (Hz).
-    // reducing it makes the gimbal yaw less responsive to vehicle yaw
-    // increasing it makes the gimbal yawe more responsive to vehicle yaw
-    float const yawRateFiltPole;
-
-    // amount of yaw angle that we permit the gimbal to lag the vehicle when operating in slave mode
-    // reducing this makes the gimbal respond more to vehicle yaw disturbances
-    float const yawErrorLimit;
-
-    uint8_t _compid;
-
-    mavlink_gimbal_report_t _report_msg;
-
-    void send_control(mavlink_channel_t chan);
-    void update_state();
-    void decode_feedback(mavlink_message_t *msg);
-
-    bool isCopterFlipped();
-
-    // Control loop functions
-    Vector3f getGimbalRateDemVecYaw(const Quaternion &quatEst);
-    Vector3f getGimbalRateDemVecTilt(const Quaternion &quatEst);
-    Vector3f getGimbalRateDemVecForward(const Quaternion &quatEst);
-    Vector3f getGimbalRateDemVecGyroBias();
-
-};
-
-#endif // AP_AHRS_NAVEKF_AVAILABLE
-
-#endif // __AP_MOUNT_H__
diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index 3671b98f51..9becf150ba 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/libraries/AP_Mount/AP_Mount.cpp
@@ -5,7 +5,7 @@
 #include "AP_Mount.h"
 #include "AP_Mount_Backend.h"
 #include "AP_Mount_Servo.h"
-#include "AP_Mount_MAVLink.h"
+#include "AP_Mount_SoloGimbal.h"
 #include "AP_Mount_Alexmos.h"
 #include "AP_Mount_SToRM32.h"
 #include "AP_Mount_SToRM32_serial.h"
@@ -200,80 +200,15 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_TYPE", 19, AP_Mount, state[0]._type, 0),
 
-    // @Param: _OFF_JNT_X
-    // @DisplayName: MAVLink Mount's roll angle offsets
-    // @Description: MAVLink Mount's roll angle offsets
-    // @Units: radians
-    // @Range: 0 0.5
-    // @User: Advanced
+    // 20 formerly _OFF_JNT
 
-    // @Param: _OFF_JNT_Y
-    // @DisplayName: MAVLink Mount's pitch angle offsets
-    // @Description: MAVLink Mount's pitch angle offsets
-    // @Units: radians
-    // @Range: 0 0.5
-    // @User: Advanced
+    // 21 formerly _OFF_ACC
 
-    // @Param: _OFF_JNT_Z
-    // @DisplayName: MAVLink Mount's yaw angle offsets
-    // @Description: MAVLink Mount's yaw angle offsets
-    // @Units: radians
-    // @Range: 0 0.5
-    // @User: Advanced
-    AP_GROUPINFO("_OFF_JNT", 20, AP_Mount, state[0]._gimbalParams.joint_angles_offsets, 0),
+    // 22 formerly _OFF_GYRO
 
-    // @Param: _OFF_ACC_X
-    // @DisplayName: MAVLink Mount's roll velocity offsets
-    // @Description: MAVLink Mount's roll velocity offsets
-    // @Units: m/s
-    // @Range: 0 2
-    // @User: Advanced
+    // 23 formerly _K_RATE
 
-    // @Param: _OFF_ACC_Y
-    // @DisplayName: MAVLink Mount's pitch velocity offsets
-    // @Description: MAVLink Mount's pitch velocity offsets
-    // @Units: m/s
-    // @Range: 0 2
-    // @User: Advanced
-
-    // @Param: _OFF_ACC_Z
-    // @DisplayName: MAVLink Mount's yaw velocity offsets
-    // @Description: MAVLink Mount's yaw velocity offsets
-    // @Units: m/s
-    // @Range: 0 2
-    // @User: Advanced
-    AP_GROUPINFO("_OFF_ACC",  21, AP_Mount, state[0]._gimbalParams.delta_velocity_offsets, 0),
-
-    // @Param: _OFF_GYRO_X
-    // @DisplayName: MAVLink Mount's roll gyro offsets
-    // @Description: MAVLink Mount's roll gyro offsets
-    // @Units: radians/sec
-    // @Range: 0 0.5
-    // @User: Advanced
-
-    // @Param: _OFF_GYRO_Y
-    // @DisplayName: MAVLink Mount's pitch gyro offsets
-    // @Description: MAVLink Mount's pitch gyro offsets
-    // @Units: radians/sec
-    // @Range: 0 0.5
-    // @User: Advanced
-
-    // @Param: _OFF_GYRO_Z
-    // @DisplayName: MAVLink Mount's yaw gyro offsets
-    // @Description: MAVLink Mount's yaw gyro offsets
-    // @Units: radians/sec
-    // @Range: 0 0.5
-    // @User: Advanced
-    AP_GROUPINFO("_OFF_GYRO", 22, AP_Mount, state[0]._gimbalParams.delta_angles_offsets, 0),
-
-    // @Param: _K_RATE
-    // @DisplayName: MAVLink Mount's rate gain
-    // @Description: MAVLink Mount's rate gain
-    // @Range: 0 10
-    // @User: Advanced
-    AP_GROUPINFO("_K_RATE", 23, AP_Mount, state[0]._gimbalParams.K_gimbalRate, 5.0f),
-
-    // 20 ~ 24 reserved for future parameters
+    // 24 is AVAILABLE
 
 #if AP_MOUNT_MAX_INSTANCES > 1
     // @Param: 2_DEFLT_MODE
@@ -475,13 +410,15 @@ AP_Mount::AP_Mount(const AP_AHRS_TYPE &ahrs, const struct Location &current_loc)
 }
 
 // init - detect and initialise all mounts
-void AP_Mount::init(const AP_SerialManager& serial_manager)
+void AP_Mount::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager)
 {
     // check init has not been called before
     if (_num_instances != 0) {
         return;
     }
 
+    _dataflash = dataflash;
+
     // default mount to servo mount if rc output channels to control roll, tilt or pan have been defined
     if (!state[0]._type.configured()) {
         if (RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_pan) ||
@@ -508,8 +445,8 @@ void AP_Mount::init(const AP_SerialManager& serial_manager)
 
 #if AP_AHRS_NAVEKF_AVAILABLE
         // check for MAVLink mounts
-        } else if (mount_type == Mount_Type_MAVLink) {
-            _backends[instance] = new AP_Mount_MAVLink(*this, state[instance], instance);
+        } else if (mount_type == Mount_Type_SoloGimbal) {
+            _backends[instance] = new AP_Mount_SoloGimbal(*this, state[instance], instance);
             _num_instances++;
 #endif
 
@@ -551,6 +488,17 @@ void AP_Mount::update()
     }
 }
 
+// used for gimbals that need to read INS data at full rate
+void AP_Mount::update_fast()
+{
+    // update each instance
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != NULL) {
+            _backends[instance]->update_fast();
+        }
+    }
+}
+
 // get_mount_type - returns the type of mount
 AP_Mount::MountType AP_Mount::get_mount_type(uint8_t instance) const
 {
@@ -674,7 +622,17 @@ void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *m
         if (_backends[instance] != NULL) {
             _backends[instance]->handle_gimbal_report(chan, msg);
         }
-    }    
+    }
+}
+
+// handle PARAM_VALUE
+void AP_Mount::handle_param_value(mavlink_message_t *msg)
+{
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != NULL) {
+            _backends[instance]->handle_param_value(msg);
+        }
+    }
 }
 
 // send a GIMBAL_REPORT message to the GCS
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index 3608319ba0..766e2dab25 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/libraries/AP_Mount/AP_Mount.h
@@ -29,6 +29,7 @@
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <RC_Channel/RC_Channel.h>
 #include <AP_SerialManager/AP_SerialManager.h>
+#include <DataFlash/DataFlash.h>
 
 // maximum number of mounts
 #define AP_MOUNT_MAX_INSTANCES          1
@@ -36,7 +37,7 @@
 // declare backend classes
 class AP_Mount_Backend;
 class AP_Mount_Servo;
-class AP_Mount_MAVLink;
+class AP_Mount_SoloGimbal;
 class AP_Mount_Alexmos;
 class AP_Mount_SToRM32;
 class AP_Mount_SToRM32_serial;
@@ -51,7 +52,7 @@ class AP_Mount
     // declare backends as friends
     friend class AP_Mount_Backend;
     friend class AP_Mount_Servo;
-    friend class AP_Mount_MAVLink;
+    friend class AP_Mount_SoloGimbal;
     friend class AP_Mount_Alexmos;
     friend class AP_Mount_SToRM32;
     friend class AP_Mount_SToRM32_serial;
@@ -62,28 +63,24 @@ public:
     enum MountType {
         Mount_Type_None = 0,            /// no mount
         Mount_Type_Servo = 1,           /// servo controlled mount
-        Mount_Type_MAVLink = 2,         /// MAVLink controlled mount
+        Mount_Type_SoloGimbal = 2,      /// Solo's gimbal
         Mount_Type_Alexmos = 3,         /// Alexmos mount
         Mount_Type_SToRM32 = 4,         /// SToRM32 mount using MAVLink protocol
         Mount_Type_SToRM32_serial = 5   /// SToRM32 mount using custom serial protocol
     };
 
-    struct gimbal_params {
-        AP_Vector3f     delta_angles_offsets;
-        AP_Vector3f     delta_velocity_offsets;
-        AP_Vector3f     joint_angles_offsets;
-        AP_Float        K_gimbalRate;
-    };
-
     // Constructor
     AP_Mount(const AP_AHRS_TYPE &ahrs, const struct Location &current_loc);
 
     // init - detect and initialise all mounts
-    void init(const AP_SerialManager& serial_manager);
+    void init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager);
 
     // update - give mount opportunity to update servos.  should be called at 10hz or higher
     void update();
 
+    // used for gimbals that need to read INS data at full rate
+    void update_fast();
+
     // get_mount_type - returns the type of mount
     AP_Mount::MountType get_mount_type() const { return get_mount_type(_primary); }
     AP_Mount::MountType get_mount_type(uint8_t instance) const;
@@ -126,6 +123,9 @@ public:
     void control_msg(mavlink_message_t* msg) { control_msg(_primary, msg); }
     void control_msg(uint8_t instance, mavlink_message_t* msg);
 
+    // handle a PARAM_VALUE message
+    void handle_param_value(mavlink_message_t *msg);
+
     // handle a GIMBAL_REPORT message
     void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);
 
@@ -182,10 +182,9 @@ protected:
 
         MAV_MOUNT_MODE  _mode;              // current mode (see MAV_MOUNT_MODE enum)
         struct Location _roi_target;        // roi target location
-
-        struct gimbal_params _gimbalParams;
-        
     } state[AP_MOUNT_MAX_INSTANCES];
+
+    DataFlash_Class *_dataflash;
 };
 
 #endif // __AP_MOUNT_H__
diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h
index 221f310fec..4795e1133d 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.h
+++ b/libraries/AP_Mount/AP_Mount_Backend.h
@@ -44,6 +44,9 @@ public:
     // update mount position - should be called periodically
     virtual void update() = 0;
 
+    // used for gimbals that need to read INS data at full rate
+    virtual void update_fast() {}
+
     // has_pan_control - returns true if this mount can control it's pan (required for multicopters)
     virtual bool has_pan_control() const = 0;
 
@@ -71,6 +74,9 @@ public:
     // handle a GIMBAL_REPORT message
     virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) {}
 
+    // handle a PARAM_VALUE message
+    virtual void handle_param_value(mavlink_message_t *msg) {}
+
     // send a GIMBAL_REPORT message to the GCS
     virtual void send_gimbal_report(mavlink_channel_t chan) {}
 
diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
similarity index 53%
rename from libraries/AP_Mount/AP_Mount_MAVLink.cpp
rename to libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
index d76b68ed94..8cd07db032 100644
--- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp
+++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
@@ -1,30 +1,40 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#include "AP_Mount_MAVLink.h"
+#include <AP_HAL/AP_HAL.h>
+#include <AP_AHRS/AP_AHRS.h>
 #if AP_AHRS_NAVEKF_AVAILABLE
+
+#include "AP_Mount_SoloGimbal.h"
+#include "SoloGimbal.h"
+#include <DataFlash/DataFlash.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
-#include <stdio.h>
-#include "AP_Gimbal.h"
 
 #if MOUNT_DEBUG
 #include <stdio.h>
 #endif
 
-AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
+extern const AP_HAL::HAL& hal;
+
+AP_Mount_SoloGimbal::AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
     AP_Mount_Backend(frontend, state, instance),
     _initialised(false),
-    _gimbal(frontend._ahrs, MAV_COMP_ID_GIMBAL, _state._gimbalParams)
+    _gimbal(frontend._ahrs)
 {}
 
 // init - performs any required initialisation for this instance
-void AP_Mount_MAVLink::init(const AP_SerialManager& serial_manager)
+void AP_Mount_SoloGimbal::init(const AP_SerialManager& serial_manager)
 {
     _initialised = true;
     set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get());
 }
 
+void AP_Mount_SoloGimbal::update_fast()
+{
+    _gimbal.update_fast();
+}
+
 // update mount position - should be called periodically
-void AP_Mount_MAVLink::update()
+void AP_Mount_SoloGimbal::update()
 {
     // exit immediately if not initialised
     if (!_initialised) {
@@ -35,25 +45,36 @@ void AP_Mount_MAVLink::update()
     switch(get_mode()) {
         // move mount to a "retracted" position.  we do not implement a separate servo based retract mechanism
         case MAV_MOUNT_MODE_RETRACT:
+            _gimbal.set_lockedToBody(true);
             break;
 
         // move mount to a neutral position, typically pointing forward
         case MAV_MOUNT_MODE_NEUTRAL:
+            {
+            _gimbal.set_lockedToBody(false);
+            const Vector3f &target = _state._neutral_angles.get();
+            _angle_ef_target_rad.x = ToRad(target.x);
+            _angle_ef_target_rad.y = ToRad(target.y);
+            _angle_ef_target_rad.z = ToRad(target.z);
+            }
             break;
 
         // point to the angles given by a mavlink message
         case MAV_MOUNT_MODE_MAVLINK_TARGETING:
+            _gimbal.set_lockedToBody(false);
             // do nothing because earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS
             break;
 
         // RC radio manual angle control, but with stabilization from the AHRS
         case MAV_MOUNT_MODE_RC_TARGETING:
+            _gimbal.set_lockedToBody(false);
             // update targets using pilot's rc inputs
             update_targets_from_rc();
             break;
 
         // point mount to a GPS point given by the mission planner
         case MAV_MOUNT_MODE_GPS_POINT:
+            _gimbal.set_lockedToBody(false);
             if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
                 calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
             }
@@ -66,14 +87,14 @@ void AP_Mount_MAVLink::update()
 }
 
 // has_pan_control - returns true if this mount can control it's pan (required for multicopters)
-bool AP_Mount_MAVLink::has_pan_control() const
+bool AP_Mount_SoloGimbal::has_pan_control() const
 {
     // we do not have yaw control
     return false;
 }
 
 // set_mode - sets mount's mode
-void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode)
+void AP_Mount_SoloGimbal::set_mode(enum MAV_MOUNT_MODE mode)
 {
     // exit immediately if not initialised
     if (!_initialised) {
@@ -85,27 +106,49 @@ void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode)
 }
 
 // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan)
+void AP_Mount_SoloGimbal::status_msg(mavlink_channel_t chan)
 {
-    Vector3f est = _gimbal.getGimbalEstimateEF();
-    mavlink_msg_mount_status_send(chan, 0, 0, degrees(est.y)*100, degrees(est.x)*100, degrees(est.z)*100);
+    if (_gimbal.aligned()) {
+        mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_ef_target_rad.y)*100, degrees(_angle_ef_target_rad.x)*100, degrees(_angle_ef_target_rad.z)*100);
+    }
 }
 
 /*
   handle a GIMBAL_REPORT message
  */
-void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg)
+void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg)
 {
     _gimbal.update_target(_angle_ef_target_rad);
     _gimbal.receive_feedback(chan,msg);
+
+    if(!_params_saved && _frontend._dataflash != NULL && _frontend._dataflash->logging_started()) {
+        _gimbal.fetch_params();       //last parameter save might not be stored in dataflash so retry
+        _params_saved = true;
+    }
+
+    if (_gimbal.get_log_dt() > 1.0f/25.0f) {
+        _gimbal.write_logs(_frontend._dataflash);
+    }
+}
+
+void AP_Mount_SoloGimbal::handle_param_value(mavlink_message_t *msg)
+{
+    _gimbal.handle_param_value(_frontend._dataflash, msg);
+}
+
+/*
+  handle a GIMBAL_REPORT message
+ */
+void AP_Mount_SoloGimbal::handle_gimbal_torque_report(mavlink_channel_t chan, mavlink_message_t *msg)
+{
+    _gimbal.disable_torque_report();
 }
 
 /*
   send a GIMBAL_REPORT message to the GCS
  */
-void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan)
+void AP_Mount_SoloGimbal::send_gimbal_report(mavlink_channel_t chan)
 {
-    _gimbal.send_report(chan);
 }
 
 #endif // AP_AHRS_NAVEKF_AVAILABLE
diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_SoloGimbal.h
similarity index 69%
rename from libraries/AP_Mount/AP_Mount_MAVLink.h
rename to libraries/AP_Mount/AP_Mount_SoloGimbal.h
index fd62a72e32..ebce708e12 100644
--- a/libraries/AP_Mount/AP_Mount_MAVLink.h
+++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.h
@@ -4,8 +4,9 @@
   MAVLink enabled mount backend class
  */
 
-#ifndef __AP_MOUNT_MAVLINK_H__
-#define __AP_MOUNT_MAVLINK_H__
+#ifndef __AP_MOUNT_SOLOGIMBAL_H__
+#define __AP_MOUNT_SOLOGIMBAL_H__
+
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_AHRS/AP_AHRS.h>
@@ -17,14 +18,15 @@
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <RC_Channel/RC_Channel.h>
 #include "AP_Mount_Backend.h"
-#include "AP_Gimbal.h"
+#include "SoloGimbal.h"
 
-class AP_Mount_MAVLink : public AP_Mount_Backend
+
+class AP_Mount_SoloGimbal : public AP_Mount_Backend
 {
 
 public:
     // Constructor
-    AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance);
+    AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance);
 
     // init - performs any required initialisation for this instance
     virtual void init(const AP_SerialManager& serial_manager);
@@ -43,17 +45,26 @@ public:
 
     // handle a GIMBAL_REPORT message
     virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);
+    virtual void handle_gimbal_torque_report(mavlink_channel_t chan, mavlink_message_t *msg);
+    virtual void handle_param_value(mavlink_message_t *msg);
 
     // send a GIMBAL_REPORT message to the GCS
     virtual void send_gimbal_report(mavlink_channel_t chan);
 
+    virtual void update_fast();
+
 private:
     // internal variables
     bool _initialised;              // true once the driver has been initialised
 
-    AP_Gimbal _gimbal;
+    // Write a gimbal measurament and estimation data packet
+    void Log_Write_Gimbal(SoloGimbal &gimbal);
+
+    bool _params_saved;
+
+    SoloGimbal _gimbal;
 };
 
 #endif // AP_AHRS_NAVEKF_AVAILABLE
 
-#endif // __AP_MOUNT_MAVLINK_H__
+#endif // __AP_MOUNT_SOLOGIMBAL_H__
diff --git a/libraries/AP_Mount/SoloGimbal.cpp b/libraries/AP_Mount/SoloGimbal.cpp
new file mode 100644
index 0000000000..a358fbf077
--- /dev/null
+++ b/libraries/AP_Mount/SoloGimbal.cpp
@@ -0,0 +1,489 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_AHRS/AP_AHRS.h>
+#if AP_AHRS_NAVEKF_AVAILABLE
+
+#include "SoloGimbal.h"
+
+#include <stdio.h>
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+bool SoloGimbal::present()
+{
+    if (_state != GIMBAL_STATE_NOT_PRESENT && AP_HAL::millis()-_last_report_msg_ms > 3000) {
+        // gimbal went away
+        _state = GIMBAL_STATE_NOT_PRESENT;
+        return false;
+    }
+
+    return _state != GIMBAL_STATE_NOT_PRESENT;
+}
+
+bool SoloGimbal::aligned()
+{
+    return present() && _state == GIMBAL_STATE_PRESENT_RUNNING;
+}
+
+gimbal_mode_t SoloGimbal::get_mode()
+{
+    if ((_gimbalParams.initialized() && _gimbalParams.get_K_rate()==0.0f) || (_ahrs.get_rotation_body_to_ned().c.z < 0 && !(_lockedToBody || _calibrator.running()))) {
+        return GIMBAL_MODE_IDLE;
+    } else if (!_ekf.getStatus()) {
+        return GIMBAL_MODE_POS_HOLD;
+    } else if (_calibrator.running() || _lockedToBody) {
+        return GIMBAL_MODE_POS_HOLD_FF;
+    } else {
+        return GIMBAL_MODE_STABILIZE;
+    }
+}
+
+void SoloGimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
+{
+    mavlink_gimbal_report_t report_msg;
+    mavlink_msg_gimbal_report_decode(msg, &report_msg);
+    uint32_t tnow_ms = AP_HAL::millis();
+    _last_report_msg_ms = tnow_ms;
+
+    _gimbalParams.set_channel(chan);
+
+    if (report_msg.target_system != 1) {
+        _state = GIMBAL_STATE_NOT_PRESENT;
+    }
+
+    switch(_state) {
+        case GIMBAL_STATE_NOT_PRESENT:
+            // gimbal was just connected or we just rebooted, transition to PRESENT_INITIALIZING
+            _gimbalParams.reset();
+            _gimbalParams.set_param(GMB_PARAM_GMB_SYSID, 1);
+            _state = GIMBAL_STATE_PRESENT_INITIALIZING;
+            break;
+
+        case GIMBAL_STATE_PRESENT_INITIALIZING:
+            _gimbalParams.update();
+            if (_gimbalParams.initialized()) {
+                // parameters done initializing, finalize initialization and transition to aligning
+                extract_feedback(report_msg);
+                _ang_vel_mag_filt = 20;
+                _filtered_joint_angles = _measurement.joint_angles;
+                _vehicle_to_gimbal_quat_filt.from_vector312(_filtered_joint_angles.x,_filtered_joint_angles.y,_filtered_joint_angles.z);
+                _ekf.reset();
+                _state = GIMBAL_STATE_PRESENT_ALIGNING;
+            }
+            break;
+
+        case GIMBAL_STATE_PRESENT_ALIGNING:
+            _gimbalParams.update();
+            extract_feedback(report_msg);
+            update_estimators();
+            if (_ekf.getStatus()) {
+                // EKF done aligning, transition to running
+                _state = GIMBAL_STATE_PRESENT_RUNNING;
+            }
+            break;
+
+        case GIMBAL_STATE_PRESENT_RUNNING:
+            _gimbalParams.update();
+            extract_feedback(report_msg);
+            update_estimators();
+            break;
+    }
+
+    send_controls(chan);
+}
+
+void SoloGimbal::send_controls(mavlink_channel_t chan)
+{
+    if (_state == GIMBAL_STATE_PRESENT_RUNNING) {
+        // get the gimbal quaternion estimate
+        Quaternion quatEst;
+        _ekf.getQuat(quatEst);
+
+        // run rate controller
+        _ang_vel_dem_rads.zero();
+        switch(get_mode()) {
+            case GIMBAL_MODE_POS_HOLD_FF: {
+                _ang_vel_dem_rads += get_ang_vel_dem_body_lock();
+                _ang_vel_dem_rads += get_ang_vel_dem_gyro_bias();
+                float _ang_vel_dem_radsLen = _ang_vel_dem_rads.length();
+                if (_ang_vel_dem_radsLen > radians(400)) {
+                    _ang_vel_dem_rads *= radians(400)/_ang_vel_dem_radsLen;
+                }
+                mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
+                                                _ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.z);
+                break;
+            }
+            case GIMBAL_MODE_STABILIZE: {
+                _ang_vel_dem_rads += get_ang_vel_dem_yaw(quatEst);
+                _ang_vel_dem_rads += get_ang_vel_dem_tilt(quatEst);
+                _ang_vel_dem_rads += get_ang_vel_dem_feedforward(quatEst);
+                _ang_vel_dem_rads += get_ang_vel_dem_gyro_bias();
+                float ang_vel_dem_norm = _ang_vel_dem_rads.length();
+                if (ang_vel_dem_norm > radians(400)) {
+                    _ang_vel_dem_rads *= radians(400)/ang_vel_dem_norm;
+                }
+                mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
+                                                _ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.z);
+                break;
+            }
+            default:
+            case GIMBAL_MODE_IDLE:
+            case GIMBAL_MODE_POS_HOLD:
+                break;
+        }
+    }
+
+    // set GMB_POS_HOLD
+    if (get_mode() == GIMBAL_MODE_POS_HOLD) {
+        _gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 1);
+    } else {
+        _gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 0);
+    }
+
+    // set GMB_MAX_TORQUE
+    float max_torque;
+    _gimbalParams.get_param(GMB_PARAM_GMB_MAX_TORQUE, max_torque, 0);
+    if (max_torque != _max_torque && max_torque != 0) {
+        _max_torque = max_torque;
+    }
+
+    if (!hal.util->get_soft_armed() || joints_near_limits()) {
+        _gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, _max_torque);
+    } else {
+        _gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, 0);
+    }
+}
+
+void SoloGimbal::extract_feedback(const mavlink_gimbal_report_t& report_msg)
+{
+    _measurement.delta_time = report_msg.delta_time;
+    _measurement.delta_angles.x = report_msg.delta_angle_x;
+    _measurement.delta_angles.y = report_msg.delta_angle_y;
+    _measurement.delta_angles.z = report_msg.delta_angle_z;
+    _measurement.delta_velocity.x = report_msg.delta_velocity_x,
+    _measurement.delta_velocity.y = report_msg.delta_velocity_y;
+    _measurement.delta_velocity.z = report_msg.delta_velocity_z;
+    _measurement.joint_angles.x = report_msg.joint_roll;
+    _measurement.joint_angles.y = report_msg.joint_el;
+    _measurement.joint_angles.z = report_msg.joint_az;
+
+    if (_calibrator.get_status() == ACCEL_CAL_COLLECTING_SAMPLE) {
+        _calibrator.new_sample(_measurement.delta_velocity,_measurement.delta_time);
+    }
+
+    _measurement.delta_angles -= _gimbalParams.get_gyro_bias() * _measurement.delta_time;
+    _measurement.joint_angles -= _gimbalParams.get_joint_bias();
+    _measurement.delta_velocity -= _gimbalParams.get_accel_bias() * _measurement.delta_time;
+    Vector3f accel_gain = _gimbalParams.get_accel_gain();
+    _measurement.delta_velocity.x *= accel_gain.x == 0.0f ? 1.0f : accel_gain.x;
+    _measurement.delta_velocity.y *= accel_gain.y == 0.0f ? 1.0f : accel_gain.y;
+    _measurement.delta_velocity.z *= accel_gain.z == 0.0f ? 1.0f : accel_gain.z;
+
+    // update _ang_vel_mag_filt, used for accel sample readiness
+    Vector3f ang_vel = _measurement.delta_angles / _measurement.delta_time;
+    Vector3f ekf_gyro_bias;
+    _ekf.getGyroBias(ekf_gyro_bias);
+    ang_vel -= ekf_gyro_bias;
+    float alpha = constrain_float(_measurement.delta_time/(_measurement.delta_time+0.5f),0.0f,1.0f);
+    _ang_vel_mag_filt += (ang_vel.length()-_ang_vel_mag_filt)*alpha;
+    _ang_vel_mag_filt = MIN(_ang_vel_mag_filt,20.0f);
+
+    // get complementary filter inputs
+    _vehicle_to_gimbal_quat.from_vector312(_measurement.joint_angles.x,_measurement.joint_angles.y,_measurement.joint_angles.z);
+
+    // update log deltas
+    _log_dt += _measurement.delta_time;
+    _log_del_ang += _measurement.delta_angles;
+    _log_del_vel += _measurement.delta_velocity;
+}
+
+void SoloGimbal::update_estimators()
+{
+    if (_state == GIMBAL_STATE_NOT_PRESENT || _state == GIMBAL_STATE_PRESENT_INITIALIZING) {
+        return;
+    }
+
+    // Run the gimbal attitude and gyro bias estimator
+    _ekf.RunEKF(_measurement.delta_time, _measurement.delta_angles, _measurement.delta_velocity, _measurement.joint_angles);
+    update_joint_angle_est();
+}
+
+void SoloGimbal::readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
+    const AP_InertialSensor &ins = _ahrs.get_ins();
+
+    if (ins_index < ins.get_gyro_count()) {
+        if (!ins.get_delta_angle(ins_index,dAng)) {
+            dAng = ins.get_gyro(ins_index) / ins.get_sample_rate();
+        }
+    }
+}
+
+void SoloGimbal::update_fast() {
+    const AP_InertialSensor &ins = _ahrs.get_ins();
+
+    if (ins.get_gyro_health(0) && ins.get_gyro_health(1)) {
+        // dual gyro mode - average first two gyros
+        Vector3f dAng;
+        readVehicleDeltaAngle(0, dAng);
+        _vehicle_delta_angles += dAng*0.5f;
+        readVehicleDeltaAngle(1, dAng);
+        _vehicle_delta_angles += dAng*0.5f;
+    } else {
+        // single gyro mode - one of the first two gyros are unhealthy or don't exist
+        // just read primary gyro
+        Vector3f dAng;
+        readVehicleDeltaAngle(ins.get_primary_gyro(), dAng);
+        _vehicle_delta_angles += dAng;
+    }
+}
+
+void SoloGimbal::update_joint_angle_est()
+{
+    static const float tc = 1.0f;
+    float dt = _measurement.delta_time;
+    float alpha = constrain_float(dt/(dt+tc),0.0f,1.0f);
+
+    Matrix3f Tvg; // vehicle frame to gimbal frame
+    _vehicle_to_gimbal_quat.inverse().rotation_matrix(Tvg);
+
+    Vector3f delta_angle_bias;
+    _ekf.getGyroBias(delta_angle_bias);
+    delta_angle_bias *= dt;
+
+    Vector3f joint_del_ang;
+    gimbal_ang_vel_to_joint_rates((_measurement.delta_angles-delta_angle_bias) - Tvg*_vehicle_delta_angles, joint_del_ang);
+
+    _filtered_joint_angles += joint_del_ang;
+    _filtered_joint_angles += (_measurement.joint_angles-_filtered_joint_angles)*alpha;
+
+    _vehicle_to_gimbal_quat_filt.from_vector312(_filtered_joint_angles.x,_filtered_joint_angles.y,_filtered_joint_angles.z);
+
+    _vehicle_delta_angles.zero();
+}
+
+Vector3f SoloGimbal::get_ang_vel_dem_yaw(const Quaternion &quatEst)
+{
+    static const float tc = 0.1f;
+    static const float yawErrorLimit = radians(5.7f);
+    float dt = _measurement.delta_time;
+    float alpha = dt/(dt+tc);
+
+    Matrix3f Tve = _ahrs.get_rotation_body_to_ned();
+    Matrix3f Teg;
+    quatEst.inverse().rotation_matrix(Teg);
+
+
+    //_vehicle_yaw_rate_ef_filt = _ahrs.get_yaw_rate_earth();
+
+    // filter the vehicle yaw rate to remove noise
+    _vehicle_yaw_rate_ef_filt += (_ahrs.get_yaw_rate_earth() - _vehicle_yaw_rate_ef_filt) * alpha;
+
+    float yaw_rate_ff = 0;
+
+    // calculate an earth-frame yaw rate feed-forward that prevents gimbal from exceeding the maximum yaw error
+    if (_vehicle_yaw_rate_ef_filt > _gimbalParams.get_K_rate()*yawErrorLimit) {
+        yaw_rate_ff = _vehicle_yaw_rate_ef_filt-_gimbalParams.get_K_rate()*yawErrorLimit;
+    } else if (_vehicle_yaw_rate_ef_filt < -_gimbalParams.get_K_rate()*yawErrorLimit) {
+        yaw_rate_ff = _vehicle_yaw_rate_ef_filt+_gimbalParams.get_K_rate()*yawErrorLimit;
+    }
+
+    // filter the feed-forward to remove noise
+    //_yaw_rate_ff_ef_filt += (yaw_rate_ff - _yaw_rate_ff_ef_filt) * alpha;
+
+    Vector3f gimbalRateDemVecYaw;
+    gimbalRateDemVecYaw.z = yaw_rate_ff - _gimbalParams.get_K_rate() * _filtered_joint_angles.z / constrain_float(Tve.c.z,0.5f,1.0f);
+    gimbalRateDemVecYaw.z /= constrain_float(Tve.c.z,0.5f,1.0f);
+
+    // rotate the rate demand into gimbal frame
+    gimbalRateDemVecYaw = Teg * gimbalRateDemVecYaw;
+
+    return gimbalRateDemVecYaw;
+}
+
+Vector3f SoloGimbal::get_ang_vel_dem_tilt(const Quaternion &quatEst)
+{
+    // Calculate the gimbal 321 Euler angle estimates relative to earth frame
+    Vector3f eulerEst = quatEst.to_vector312();
+
+    // Calculate a demanded quaternion using the demanded roll and pitch and estimated yaw (yaw is slaved to the vehicle)
+    Quaternion quatDem;
+    quatDem.from_vector312( _att_target_euler_rad.x,
+                            _att_target_euler_rad.y,
+                            eulerEst.z);
+
+    //divide the demanded quaternion by the estimated to get the error
+    Quaternion quatErr = quatDem / quatEst;
+
+    // Convert to a delta rotation
+    quatErr.normalize();
+    Vector3f deltaAngErr;
+    quatErr.to_axis_angle(deltaAngErr);
+
+    // multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt
+    Vector3f gimbalRateDemVecTilt = deltaAngErr * _gimbalParams.get_K_rate();
+    return gimbalRateDemVecTilt;
+}
+
+Vector3f SoloGimbal::get_ang_vel_dem_feedforward(const Quaternion &quatEst)
+{
+    // quaternion demanded at the previous time step
+    static float lastDem;
+
+    // calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation
+    float delta = _att_target_euler_rad.y - lastDem;
+    lastDem = _att_target_euler_rad.y;
+
+    Vector3f gimbalRateDemVecForward;
+    gimbalRateDemVecForward.y = delta / _measurement.delta_time;
+    return gimbalRateDemVecForward;
+}
+
+Vector3f SoloGimbal::get_ang_vel_dem_gyro_bias()
+{
+    Vector3f gyroBias;
+    _ekf.getGyroBias(gyroBias);
+    return gyroBias + _gimbalParams.get_gyro_bias();
+}
+
+Vector3f SoloGimbal::get_ang_vel_dem_body_lock()
+{
+    // Define rotation from vehicle to gimbal using a 312 rotation sequence
+    Matrix3f Tvg;
+    _vehicle_to_gimbal_quat_filt.inverse().rotation_matrix(Tvg);
+
+    // multiply the joint angles by a gain to calculate a rate vector required to keep the joints centred
+    Vector3f gimbalRateDemVecBodyLock;
+    gimbalRateDemVecBodyLock = _filtered_joint_angles * -_gimbalParams.get_K_rate();
+
+    joint_rates_to_gimbal_ang_vel(gimbalRateDemVecBodyLock, gimbalRateDemVecBodyLock);
+
+    // Add a feedforward term from vehicle gyros
+    gimbalRateDemVecBodyLock += Tvg * _ahrs.get_gyro();
+
+    return gimbalRateDemVecBodyLock;
+}
+
+void SoloGimbal::update_target(Vector3f newTarget)
+{
+    // Low-pass filter
+    _att_target_euler_rad.y = _att_target_euler_rad.y + 0.02f*(newTarget.y - _att_target_euler_rad.y);
+    // Update tilt
+    _att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y,radians(-90),radians(0));
+}
+
+void SoloGimbal::write_logs(DataFlash_Class* dataflash)
+{
+    if (dataflash == NULL) return;
+
+    uint32_t tstamp = AP_HAL::millis();
+    Vector3f eulerEst;
+
+    Quaternion quatEst;
+    _ekf.getQuat(quatEst);
+    quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
+
+    struct log_Gimbal1 pkt1 = {
+        LOG_PACKET_HEADER_INIT(LOG_GIMBAL1_MSG),
+        time_ms : tstamp,
+        delta_time      : _log_dt,
+        delta_angles_x  : _log_del_ang.x,
+        delta_angles_y  : _log_del_ang.y,
+        delta_angles_z  : _log_del_ang.z,
+        delta_velocity_x : _log_del_vel.x,
+        delta_velocity_y : _log_del_vel.y,
+        delta_velocity_z : _log_del_vel.z,
+        joint_angles_x  : _measurement.joint_angles.x,
+        joint_angles_y  : _measurement.joint_angles.y,
+        joint_angles_z  : _measurement.joint_angles.z
+    };
+    dataflash->WriteBlock(&pkt1, sizeof(pkt1));
+
+    struct log_Gimbal2 pkt2 = {
+        LOG_PACKET_HEADER_INIT(LOG_GIMBAL2_MSG),
+        time_ms : tstamp,
+        est_sta : (uint8_t) _ekf.getStatus(),
+        est_x   : eulerEst.x,
+        est_y   : eulerEst.y,
+        est_z   : eulerEst.z,
+        rate_x  : _ang_vel_dem_rads.x,
+        rate_y  : _ang_vel_dem_rads.y,
+        rate_z  : _ang_vel_dem_rads.z,
+        target_x: _att_target_euler_rad.x,
+        target_y: _att_target_euler_rad.y,
+        target_z: _att_target_euler_rad.z
+    };
+    dataflash->WriteBlock(&pkt2, sizeof(pkt2));
+
+    _log_dt = 0;
+    _log_del_ang.zero();
+    _log_del_vel.zero();
+}
+
+bool SoloGimbal::joints_near_limits()
+{
+    return fabsf(_measurement.joint_angles.x) > radians(40) || _measurement.joint_angles.y > radians(45) || _measurement.joint_angles.y < -radians(135);
+}
+
+AccelCalibrator* SoloGimbal::_acal_get_calibrator(uint8_t instance)
+{
+    if(instance==0 && (present() || _calibrator.get_status() == ACCEL_CAL_SUCCESS)) {
+        return &_calibrator;
+    } else {
+        return NULL;
+    }
+}
+
+bool SoloGimbal::_acal_get_ready_to_sample()
+{
+    return _ang_vel_mag_filt < radians(10);
+}
+
+bool SoloGimbal::_acal_get_saving()
+{
+    return _gimbalParams.flashing();
+}
+
+void SoloGimbal::_acal_save_calibrations()
+{
+    if (_calibrator.get_status() != ACCEL_CAL_SUCCESS) {
+        return;
+    }
+    Vector3f bias;
+    Vector3f gain;
+    _calibrator.get_calibration(bias,gain);
+    _gimbalParams.set_accel_bias(bias);
+    _gimbalParams.set_accel_gain(gain);
+    _gimbalParams.flash();
+}
+
+void SoloGimbal::gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates)
+{
+    float sin_theta = sinf(_measurement.joint_angles.y);
+    float cos_theta = cosf(_measurement.joint_angles.y);
+
+    float sin_phi = sinf(_measurement.joint_angles.x);
+    float cos_phi = cosf(_measurement.joint_angles.x);
+    float sec_phi = 1.0f/cos_phi;
+    float tan_phi = sin_phi/cos_phi;
+
+    joint_rates.x = ang_vel.x*cos_theta+ang_vel.z*sin_theta;
+    joint_rates.y = ang_vel.x*sin_theta*tan_phi-ang_vel.z*cos_theta*tan_phi+ang_vel.y;
+    joint_rates.z = sec_phi*(ang_vel.z*cos_theta-ang_vel.x*sin_theta);
+}
+
+void SoloGimbal::joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel)
+{
+    float sin_theta = sinf(_measurement.joint_angles.y);
+    float cos_theta = cosf(_measurement.joint_angles.y);
+
+    float sin_phi = sinf(_measurement.joint_angles.x);
+    float cos_phi = cosf(_measurement.joint_angles.x);
+
+    ang_vel.x = cos_theta*joint_rates.x-sin_theta*cos_phi*joint_rates.z;
+    ang_vel.y = joint_rates.y + sin_phi*joint_rates.z;
+    ang_vel.z = sin_theta*joint_rates.x+cos_theta*cos_phi*joint_rates.z;
+}
+
+#endif // AP_AHRS_NAVEKF_AVAILABLE
diff --git a/libraries/AP_Mount/SoloGimbal.h b/libraries/AP_Mount/SoloGimbal.h
new file mode 100644
index 0000000000..e195505e33
--- /dev/null
+++ b/libraries/AP_Mount/SoloGimbal.h
@@ -0,0 +1,160 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/************************************************************
+* SoloGimbal -- library to control a 3 axis rate gimbal.     *
+*                                                           *
+* Author:  Arthur Benemann, Paul Riseborough;               *
+*                                                           *
+************************************************************/
+#ifndef __SOLOGIMBAL_H__
+#define __SOLOGIMBAL_H__
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_AHRS/AP_AHRS.h>
+#if AP_AHRS_NAVEKF_AVAILABLE
+
+#include "AP_Mount.h"
+#include "SoloGimbalEKF.h"
+#include <AP_Math/AP_Math.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_GPS/AP_GPS.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_NavEKF/AP_NavEKF.h>
+#include <AP_AccelCal/AP_AccelCal.h>
+
+#include "SoloGimbal_Parameters.h"
+
+enum gimbal_state_t {
+    GIMBAL_STATE_NOT_PRESENT = 0,
+    GIMBAL_STATE_PRESENT_INITIALIZING,
+    GIMBAL_STATE_PRESENT_ALIGNING,
+    GIMBAL_STATE_PRESENT_RUNNING
+};
+
+enum gimbal_mode_t {
+    GIMBAL_MODE_IDLE = 0,
+    GIMBAL_MODE_POS_HOLD,
+    GIMBAL_MODE_POS_HOLD_FF,
+    GIMBAL_MODE_STABILIZE
+};
+
+class SoloGimbal : AP_AccelCal_Client
+{
+public:
+    //Constructor
+    SoloGimbal(const AP_AHRS_NavEKF &ahrs) :
+        _ekf(ahrs),
+        _ahrs(ahrs),
+        _state(GIMBAL_STATE_NOT_PRESENT),
+        _yaw_rate_ff_ef_filt(0.0f),
+        _vehicle_yaw_rate_ef_filt(0.0f),
+        _lockedToBody(false),
+        _vehicle_delta_angles(),
+        _vehicle_to_gimbal_quat(),
+        _vehicle_to_gimbal_quat_filt(),
+        _filtered_joint_angles(),
+        _max_torque(5000.0f),
+        _log_dt(0),
+        _log_del_ang(),
+        _log_del_vel()
+    {
+        AP_AccelCal::register_client(this);
+    }
+
+    void    update_target(Vector3f newTarget);
+    void    receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
+
+    void update_fast();
+
+    bool present();
+    bool aligned();
+
+    void set_lockedToBody(bool val) { _lockedToBody = val; }
+
+    void write_logs(DataFlash_Class* dataflash);
+
+    float get_log_dt() { return _log_dt; }
+
+    void disable_torque_report() { _gimbalParams.set_param(GMB_PARAM_GMB_SND_TORQUE, 0); }
+    void fetch_params() { _gimbalParams.fetch_params(); }
+
+    void handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg) {
+        _gimbalParams.handle_param_value(dataflash, msg);
+    }
+
+private:
+    // private methods
+    void update_estimators();
+    void send_controls(mavlink_channel_t chan);
+    void extract_feedback(const mavlink_gimbal_report_t& report_msg);
+    void update_joint_angle_est();
+
+    Vector3f get_ang_vel_dem_yaw(const Quaternion &quatEst);
+    Vector3f get_ang_vel_dem_tilt(const Quaternion &quatEst);
+    Vector3f get_ang_vel_dem_feedforward(const Quaternion &quatEst);
+    Vector3f get_ang_vel_dem_gyro_bias();
+    Vector3f get_ang_vel_dem_body_lock();
+
+    void gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates);
+    void joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel);
+
+    void readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng);
+
+    void _acal_save_calibrations();
+    bool _acal_get_ready_to_sample();
+    bool _acal_get_saving();
+    AccelCalibrator* _acal_get_calibrator(uint8_t instance);
+
+    gimbal_mode_t get_mode();
+
+    bool joints_near_limits();
+
+    // private member variables
+    gimbal_state_t _state;
+
+    struct {
+        float delta_time;
+        Vector3f delta_angles;
+        Vector3f delta_velocity;
+        Vector3f joint_angles;
+    } _measurement;
+
+    float _yaw_rate_ff_ef_filt;
+    float _vehicle_yaw_rate_ef_filt;
+
+    static const uint8_t _compid = MAV_COMP_ID_GIMBAL;
+
+    // joint angle filter states
+    Vector3f _vehicle_delta_angles;
+
+    Quaternion _vehicle_to_gimbal_quat;
+    Quaternion _vehicle_to_gimbal_quat_filt;
+    Vector3f _filtered_joint_angles;
+
+    uint32_t _last_report_msg_ms;
+
+    float _max_torque;
+
+    float _ang_vel_mag_filt;
+
+    mavlink_channel_t _chan;
+
+    Vector3f    _ang_vel_dem_rads;       // rad/s
+    Vector3f    _att_target_euler_rad;   // desired earth-frame roll, tilt and pan angles in radians
+
+    bool _lockedToBody;
+
+    SoloGimbalEKF    _ekf;                   // state of small EKF for gimbal
+    const AP_AHRS_NavEKF    &_ahrs;     //  Main EKF
+    SoloGimbal_Parameters _gimbalParams;
+
+    AccelCalibrator _calibrator;
+
+    float _log_dt;
+    Vector3f _log_del_ang;
+    Vector3f _log_del_vel;
+};
+
+#endif // AP_AHRS_NAVEKF_AVAILABLE
+
+#endif // __AP_MOUNT_H__
diff --git a/libraries/AP_Mount/SoloGimbal_Parameters.cpp b/libraries/AP_Mount/SoloGimbal_Parameters.cpp
new file mode 100644
index 0000000000..fddb388d9a
--- /dev/null
+++ b/libraries/AP_Mount/SoloGimbal_Parameters.cpp
@@ -0,0 +1,278 @@
+#include "SoloGimbal_Parameters.h"
+#include <AP_HAL/AP_HAL.h>
+
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+const uint32_t SoloGimbal_Parameters::_retry_period = 3000;
+const uint8_t SoloGimbal_Parameters::_max_fetch_attempts = 5;
+
+SoloGimbal_Parameters::SoloGimbal_Parameters()
+{
+    reset();
+}
+
+
+void SoloGimbal_Parameters::reset()
+{
+    memset(_params,0,sizeof(_params));
+    _last_request_ms = 0;
+    _flashing_step = GMB_PARAM_NOT_FLASHING;
+}
+
+const char* SoloGimbal_Parameters::get_param_name(gmb_param_t param)
+{
+    switch(param) {
+        case GMB_PARAM_GMB_OFF_ACC_X:
+            return "GMB_OFF_ACC_X";
+        case GMB_PARAM_GMB_OFF_ACC_Y:
+            return "GMB_OFF_ACC_Y";
+        case GMB_PARAM_GMB_OFF_ACC_Z:
+            return "GMB_OFF_ACC_Z";
+        case GMB_PARAM_GMB_GN_ACC_X:
+            return "GMB_GN_ACC_X";
+        case GMB_PARAM_GMB_GN_ACC_Y:
+            return "GMB_GN_ACC_Y";
+        case GMB_PARAM_GMB_GN_ACC_Z:
+            return "GMB_GN_ACC_Z";
+        case GMB_PARAM_GMB_OFF_GYRO_X:
+            return "GMB_OFF_GYRO_X";
+        case GMB_PARAM_GMB_OFF_GYRO_Y:
+            return "GMB_OFF_GYRO_Y";
+        case GMB_PARAM_GMB_OFF_GYRO_Z:
+            return "GMB_OFF_GYRO_Z";
+        case GMB_PARAM_GMB_OFF_JNT_X:
+            return "GMB_OFF_JNT_X";
+        case GMB_PARAM_GMB_OFF_JNT_Y:
+            return "GMB_OFF_JNT_Y";
+        case GMB_PARAM_GMB_OFF_JNT_Z:
+            return "GMB_OFF_JNT_Z";
+        case GMB_PARAM_GMB_K_RATE:
+            return "GMB_K_RATE";
+        case GMB_PARAM_GMB_POS_HOLD:
+            return "GMB_POS_HOLD";
+        case GMB_PARAM_GMB_MAX_TORQUE:
+            return "GMB_MAX_TORQUE";
+        case GMB_PARAM_GMB_SND_TORQUE:
+            return "GMB_SND_TORQUE";
+        case GMB_PARAM_GMB_SYSID:
+            return "GMB_SYSID";
+        case GMB_PARAM_GMB_FLASH:
+            return "GMB_FLASH";
+        default:
+            return "";
+    };
+}
+
+void SoloGimbal_Parameters::fetch_params()
+{
+    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
+        if (_params[i].state != GMB_PARAMSTATE_NOT_YET_READ) {
+            _params[i].state = GMB_PARAMSTATE_FETCH_AGAIN;
+        }
+    }
+}
+
+bool SoloGimbal_Parameters::initialized()
+{
+    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
+        if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ) {
+            return false;
+        }
+    }
+    return true;
+}
+
+bool SoloGimbal_Parameters::received_all()
+{
+    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
+        if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ || _params[i].state == GMB_PARAMSTATE_FETCH_AGAIN) {
+            return false;
+        }
+    }
+    return true;
+}
+
+void SoloGimbal_Parameters::get_param(gmb_param_t param, float& value, float def_val) {
+    if (!_params[param].seen) {
+        value = def_val;
+    } else {
+        value = _params[param].value;
+    }
+}
+
+void SoloGimbal_Parameters::set_param(gmb_param_t param, float value) {
+    if ((_params[param].state == GMB_PARAMSTATE_CONSISTENT && param != GMB_PARAM_GMB_FLASH && _params[param].value == value) || _params[param].state == GMB_PARAMSTATE_NONEXISTANT) {
+        return;
+    }
+
+    _params[param].state = GMB_PARAMSTATE_ATTEMPTING_TO_SET;
+    _params[param].value = value;
+    mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, get_param_name(param), _params[param].value, MAV_PARAM_TYPE_REAL32);
+
+    _last_request_ms = AP_HAL::millis();
+}
+
+void SoloGimbal_Parameters::update()
+{
+    uint32_t tnow_ms = AP_HAL::millis();
+
+    // retry initial param retrieval
+    if(!received_all()){
+        if (tnow_ms-_last_request_ms > _retry_period) {
+            _last_request_ms = tnow_ms;
+            mavlink_msg_param_request_list_send(_chan, 0, MAV_COMP_ID_GIMBAL);
+
+            for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
+                if (!_params[i].seen) {
+                    _params[i].fetch_attempts++;
+                }
+            }
+        }
+    }
+
+    // retry param_set
+    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
+        if (_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET && tnow_ms - _last_request_ms > _retry_period) {
+            mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, get_param_name((gmb_param_t)i), _params[i].value, MAV_PARAM_TYPE_REAL32);
+            if (!_params[i].seen) {
+                _params[i].fetch_attempts++;
+            }
+        }
+    }
+
+    // check for nonexistant parameters
+    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
+        if (!_params[i].seen && _params[i].fetch_attempts > _max_fetch_attempts) {
+            _params[i].state = GMB_PARAMSTATE_NONEXISTANT;
+            hal.console->printf("Gimbal parameter %s timed out\n", get_param_name((gmb_param_t)i));
+        }
+    }
+
+    if(_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_SET) {
+        bool done = true;
+        for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
+            if (_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET) {
+                done = false;
+                break;
+            }
+        }
+
+        if (done) {
+            _flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_ACK;
+            set_param(GMB_PARAM_GMB_FLASH,69.0f);
+        }
+    }
+}
+
+void SoloGimbal_Parameters::handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg)
+{
+    mavlink_param_value_t packet;
+    mavlink_msg_param_value_decode(msg, &packet);
+
+    if (dataflash != NULL) {
+        dataflash->Log_Write_Parameter(packet.param_id, packet.param_value);
+    }
+
+    for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
+        if (!strcmp(packet.param_id, get_param_name((gmb_param_t)i))) {
+            _params[i].seen = true;
+            switch(_params[i].state) {
+                case GMB_PARAMSTATE_NONEXISTANT:
+                case GMB_PARAMSTATE_NOT_YET_READ:
+                case GMB_PARAMSTATE_FETCH_AGAIN:
+                    _params[i].value = packet.param_value;
+                    _params[i].state = GMB_PARAMSTATE_CONSISTENT;
+                    break;
+                case GMB_PARAMSTATE_CONSISTENT:
+                    _params[i].value = packet.param_value;
+                    break;
+                case GMB_PARAMSTATE_ATTEMPTING_TO_SET:
+                    if (i == GMB_PARAM_GMB_FLASH) {
+                        if (_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_ACK && packet.param_value == 1) {
+                            _flashing_step = GMB_PARAM_NOT_FLASHING;
+                        }
+                        _params[i].value = 0;
+                        _params[i].state = GMB_PARAMSTATE_CONSISTENT;
+                    } else if (packet.param_value == _params[i].value) {
+                        _params[i].state = GMB_PARAMSTATE_CONSISTENT;
+                    }
+                    break;
+            }
+            break;
+        }
+    }
+}
+
+Vector3f SoloGimbal_Parameters::get_accel_bias()
+{
+    Vector3f ret;
+    get_param(GMB_PARAM_GMB_OFF_ACC_X,ret.x);
+    get_param(GMB_PARAM_GMB_OFF_ACC_Y,ret.y);
+    get_param(GMB_PARAM_GMB_OFF_ACC_Z,ret.z);
+    return ret;
+}
+Vector3f SoloGimbal_Parameters::get_accel_gain()
+{
+    Vector3f ret;
+    get_param(GMB_PARAM_GMB_GN_ACC_X,ret.x,1.0f);
+    get_param(GMB_PARAM_GMB_GN_ACC_Y,ret.y,1.0f);
+    get_param(GMB_PARAM_GMB_GN_ACC_Z,ret.z,1.0f);
+    return ret;
+}
+
+void SoloGimbal_Parameters::set_accel_bias(const Vector3f& bias)
+{
+    set_param(GMB_PARAM_GMB_OFF_ACC_X, bias.x);
+    set_param(GMB_PARAM_GMB_OFF_ACC_Y, bias.y);
+    set_param(GMB_PARAM_GMB_OFF_ACC_Z, bias.z);
+}
+
+void SoloGimbal_Parameters::set_accel_gain(const Vector3f& gain)
+{
+    set_param(GMB_PARAM_GMB_GN_ACC_X, gain.x);
+    set_param(GMB_PARAM_GMB_GN_ACC_Y, gain.y);
+    set_param(GMB_PARAM_GMB_GN_ACC_Z, gain.z);
+}
+
+Vector3f SoloGimbal_Parameters::get_gyro_bias()
+{
+    Vector3f ret;
+    get_param(GMB_PARAM_GMB_OFF_GYRO_X,ret.x);
+    get_param(GMB_PARAM_GMB_OFF_GYRO_Y,ret.y);
+    get_param(GMB_PARAM_GMB_OFF_GYRO_Z,ret.z);
+    return ret;
+}
+
+void SoloGimbal_Parameters::set_gyro_bias(const Vector3f& bias)
+{
+    set_param(GMB_PARAM_GMB_OFF_GYRO_X,bias.x);
+    set_param(GMB_PARAM_GMB_OFF_GYRO_Y,bias.y);
+    set_param(GMB_PARAM_GMB_OFF_GYRO_Z,bias.z);
+}
+
+Vector3f SoloGimbal_Parameters::get_joint_bias()
+{
+    Vector3f ret;
+    get_param(GMB_PARAM_GMB_OFF_JNT_X,ret.x);
+    get_param(GMB_PARAM_GMB_OFF_JNT_Y,ret.y);
+    get_param(GMB_PARAM_GMB_OFF_JNT_Z,ret.z);
+    return ret;
+}
+float SoloGimbal_Parameters::get_K_rate()
+{
+    float ret;
+    get_param(GMB_PARAM_GMB_K_RATE,ret);
+    return ret;
+}
+
+void SoloGimbal_Parameters::flash()
+{
+    _flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_SET;
+}
+
+bool SoloGimbal_Parameters::flashing()
+{
+    return _flashing_step != GMB_PARAM_NOT_FLASHING;
+}
diff --git a/libraries/AP_Mount/SoloGimbal_Parameters.h b/libraries/AP_Mount/SoloGimbal_Parameters.h
new file mode 100644
index 0000000000..76d8ce29c7
--- /dev/null
+++ b/libraries/AP_Mount/SoloGimbal_Parameters.h
@@ -0,0 +1,94 @@
+#ifndef __SOLOGIMBAL_PARAMETERS__
+#define __SOLOGIMBAL_PARAMETERS__
+#include <AP_Math/AP_Math.h>
+#include <AP_Common/AP_Common.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <DataFlash/DataFlash.h>
+
+enum gmb_param_state_t {
+    GMB_PARAMSTATE_NOT_YET_READ=0, // parameter has yet to be initialized
+    GMB_PARAMSTATE_FETCH_AGAIN=1, // parameter is being fetched
+    GMB_PARAMSTATE_ATTEMPTING_TO_SET=2, // parameter is being set
+    GMB_PARAMSTATE_CONSISTENT=3, // parameter is consistent
+    GMB_PARAMSTATE_NONEXISTANT=4 // parameter does not seem to exist
+};
+
+enum gmb_param_t {
+    GMB_PARAM_GMB_OFF_ACC_X=0,
+    GMB_PARAM_GMB_OFF_ACC_Y,
+    GMB_PARAM_GMB_OFF_ACC_Z,
+    GMB_PARAM_GMB_GN_ACC_X,
+    GMB_PARAM_GMB_GN_ACC_Y,
+    GMB_PARAM_GMB_GN_ACC_Z,
+    GMB_PARAM_GMB_OFF_GYRO_X,
+    GMB_PARAM_GMB_OFF_GYRO_Y,
+    GMB_PARAM_GMB_OFF_GYRO_Z,
+    GMB_PARAM_GMB_OFF_JNT_X,
+    GMB_PARAM_GMB_OFF_JNT_Y,
+    GMB_PARAM_GMB_OFF_JNT_Z,
+    GMB_PARAM_GMB_K_RATE,
+    GMB_PARAM_GMB_POS_HOLD,
+    GMB_PARAM_GMB_MAX_TORQUE,
+    GMB_PARAM_GMB_SND_TORQUE,
+    GMB_PARAM_GMB_SYSID,
+    GMB_PARAM_GMB_FLASH,
+    MAVLINK_GIMBAL_NUM_TRACKED_PARAMS
+};
+
+enum gmb_flashing_step_t {
+    GMB_PARAM_NOT_FLASHING=0,
+    GMB_PARAM_FLASHING_WAITING_FOR_SET,
+    GMB_PARAM_FLASHING_WAITING_FOR_ACK
+};
+
+class SoloGimbal_Parameters
+{
+public:
+    SoloGimbal_Parameters();
+    void reset();
+
+    bool initialized();
+    bool received_all();
+    void fetch_params();
+
+    void get_param(gmb_param_t param, float& value, float def_val = 0.0f);
+    void set_param(gmb_param_t param, float value);
+
+    void update();
+    void handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg);
+
+    Vector3f get_accel_bias();
+    Vector3f get_accel_gain();
+    void set_accel_bias(const Vector3f& bias);
+    void set_accel_gain(const Vector3f& gain);
+    Vector3f get_gyro_bias();
+    void set_gyro_bias(const Vector3f& bias);
+    Vector3f get_joint_bias();
+
+    float get_K_rate();
+    void flash();
+    bool flashing();
+
+    void set_channel(mavlink_channel_t chan) { _chan = chan; }
+
+private:
+    static const char* get_param_name(gmb_param_t param);
+
+    static const uint32_t _retry_period;
+    static const uint8_t _max_fetch_attempts;
+
+    struct {
+        float value;
+        gmb_param_state_t state;
+        uint8_t fetch_attempts;
+        bool seen;
+    } _params[MAVLINK_GIMBAL_NUM_TRACKED_PARAMS];
+
+    uint32_t _last_request_ms;
+    gmb_flashing_step_t _flashing_step;
+
+    mavlink_channel_t _chan;
+};
+
+
+#endif // __SOLOGIMBAL_PARAMETERS__

From 22c339765701bf6488dc3f293e2b8dfe2ef4b207 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 4 Jan 2016 21:20:48 -0800
Subject: [PATCH 21/29] Copter: make AP_Mount calls required by
 AP_Mount_SoloGimbal

---
 ArduCopter/ArduCopter.cpp  | 3 +++
 ArduCopter/GCS_Mavlink.cpp | 6 ++++++
 ArduCopter/system.cpp      | 2 +-
 3 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp
index c1d129245a..9e28146c3b 100644
--- a/ArduCopter/ArduCopter.cpp
+++ b/ArduCopter/ArduCopter.cpp
@@ -284,6 +284,9 @@ void Copter::fast_loop()
     // check if we've landed or crashed
     update_land_and_crash_detectors();
 
+    // camera mount's fast update
+    camera_mount.update_fast();
+
     // log sensor health
     if (should_log(MASK_LOG_ANY)) {
         Log_Sensor_Health();
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index d7d1103f99..b522de067f 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -1056,6 +1056,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
     }
 
+    case MAVLINK_MSG_ID_PARAM_VALUE:
+    {
+        copter.camera_mount.handle_param_value(msg);
+        break;
+    }
+
     case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
     {
         handle_mission_write_partial_list(copter.mission, msg);
diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp
index d8f3fe5a40..963a670c40 100644
--- a/ArduCopter/system.cpp
+++ b/ArduCopter/system.cpp
@@ -207,7 +207,7 @@ void Copter::init_ardupilot()
 
 #if MOUNT == ENABLED
     // initialise camera mount
-    camera_mount.init(serial_manager);
+    camera_mount.init(&DataFlash, serial_manager);
 #endif
 
 #if PRECISION_LANDING == ENABLED

From ac3b5a44005d6686c12ce05fd36dc4b9315949f4 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 4 Jan 2016 21:21:31 -0800
Subject: [PATCH 22/29] GCS_MAVLink: add messages to routing switch statement

---
 libraries/GCS_MAVLink/MAVLink_routing.cpp | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp
index f8c7b28959..720748f22b 100644
--- a/libraries/GCS_MAVLink/MAVLink_routing.cpp
+++ b/libraries/GCS_MAVLink/MAVLink_routing.cpp
@@ -493,6 +493,18 @@ void MAVLink_routing::get_targets(const mavlink_message_t* msg, int16_t &sysid,
         sysid  = mavlink_msg_gimbal_control_get_target_system(msg);
         compid = mavlink_msg_gimbal_control_get_target_component(msg);
         break;
+    case MAVLINK_MSG_ID_GIMBAL_TORQUE_CMD_REPORT:
+        sysid  = mavlink_msg_gimbal_torque_cmd_report_get_target_system(msg);
+        compid = mavlink_msg_gimbal_torque_cmd_report_get_target_component(msg);
+        break;
+    case MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK:
+        sysid  = mavlink_msg_remote_log_data_block_get_target_system(msg);
+        compid = mavlink_msg_remote_log_data_block_get_target_component(msg);
+        break;
+    case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS:
+        sysid  = mavlink_msg_remote_log_block_status_get_target_system(msg);
+        compid = mavlink_msg_remote_log_block_status_get_target_component(msg);
+        break;
     }
 }
 

From f1db10c337d48cdb10789c2e1615c6c354b14dc7 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 4 Jan 2016 21:23:11 -0800
Subject: [PATCH 23/29] APMrover2: reflect changes to AP_Mount

---
 APMrover2/system.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index b7d9edb03f..8b95a51dae 100644
--- a/APMrover2/system.cpp
+++ b/APMrover2/system.cpp
@@ -167,7 +167,7 @@ void Rover::init_ardupilot()
 
 #if MOUNT == ENABLED
     // initialise camera mount
-    camera_mount.init(serial_manager);
+    camera_mount.init(&DataFlash, serial_manager);
 #endif
 
     /*

From 9000756c80431d61fb97cbf031e47c2f0915dbbc Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 4 Jan 2016 21:23:47 -0800
Subject: [PATCH 24/29] Plane: reflect changes to AP_Mount

---
 ArduPlane/system.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index 91791dee54..bc7290873a 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -201,7 +201,7 @@ void Plane::init_ardupilot()
 
 #if MOUNT == ENABLED
     // initialise camera mount
-    camera_mount.init(serial_manager);
+    camera_mount.init(&DataFlash, serial_manager);
 #endif
 
 #if FENCE_TRIGGERED_PIN > 0

From 27ed9e15616196f89b8ae180e4964e5fdfc09ef1 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 14 Jan 2016 12:16:42 +0900
Subject: [PATCH 25/29] Copter: allow mount to be disabled

---
 ArduCopter/ArduCopter.cpp  | 2 ++
 ArduCopter/GCS_Mavlink.cpp | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp
index 9e28146c3b..31c46a63d1 100644
--- a/ArduCopter/ArduCopter.cpp
+++ b/ArduCopter/ArduCopter.cpp
@@ -284,8 +284,10 @@ void Copter::fast_loop()
     // check if we've landed or crashed
     update_land_and_crash_detectors();
 
+#if MOUNT == ENABLED
     // camera mount's fast update
     camera_mount.update_fast();
+#endif
 
     // log sensor health
     if (should_log(MASK_LOG_ANY)) {
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index b522de067f..1784d200f6 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -1058,7 +1058,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
     case MAVLINK_MSG_ID_PARAM_VALUE:
     {
+#if MOUNT == ENABLED
         copter.camera_mount.handle_param_value(msg);
+#endif
         break;
     }
 

From e78595bf4811c49688e0fc5909f1b90f4e9a2135 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 23 Jan 2016 11:42:13 +0900
Subject: [PATCH 26/29] Frame_Params: remove defaults from Bebop2 params

Removed parameter values that should already be these values by default on a fresh install
---
 Tools/Frame_params/Parrot_Bebop2.param | 19 +------------------
 1 file changed, 1 insertion(+), 18 deletions(-)

diff --git a/Tools/Frame_params/Parrot_Bebop2.param b/Tools/Frame_params/Parrot_Bebop2.param
index 538110ba09..d7c8e76c3c 100644
--- a/Tools/Frame_params/Parrot_Bebop2.param
+++ b/Tools/Frame_params/Parrot_Bebop2.param
@@ -1,7 +1,5 @@
-#NOTE: 17/01/2016 5:01:51 PM Frame : 
+#NOTE: Bebop2 parameter defaults (Copter-3.4)
 ARMING_CHECK,126
-ACRO_YAW_P,4.5
-ANGLE_MAX,4500
 ATC_ACCEL_P_MAX,220000
 ATC_ACCEL_R_MAX,220000
 ATC_ACCEL_Y_MAX,56000
@@ -10,35 +8,20 @@ BATT_CAPACITY,2700
 COMPASS_EXTERNAL,1
 COMPASS_ORIENT,2
 FRAME,3
-ATC_RATE_FF_ENAB,1
-ATC_SLEW_YAW,1000
 INS_ACCEL_FILTER,20
 INS_GYRO_FILTER,20
-MOT_CURR_MAX,0
-MOT_SPIN_ARMED,70
-MOT_THR_MIX_MAX,0.5
-MOT_THR_MIX_MIN,0.1
-MOT_THST_BAT_MAX,0
-MOT_THST_BAT_MIN,0
 MOT_THST_EXPO,0.3
 MOT_THST_MAX,1
-MOT_YAW_HEADROOM,200
 RATE_PIT_D,0.0007353554
-RATE_PIT_FILT_HZ,20
 RATE_PIT_I,0.09863888
-RATE_PIT_IMAX,2000
 RATE_PIT_P,0.09863888
 RATE_RLL_D,0.0004876749
-RATE_RLL_FILT_HZ,20
 RATE_RLL_I,0.1183951
-RATE_RLL_IMAX,2000
 RATE_RLL_P,0.1183951
 RATE_YAW_D,0
 RATE_YAW_FILT_HZ,1.001376
 RATE_YAW_I,0.09345812
-RATE_YAW_IMAX,1000
 RATE_YAW_P,0.9345812
-RC_FEEL_RP,50
 STB_PIT_P,15
 STB_RLL_P,15
 STB_YAW_P,14.25551

From 869b5e96b156f03149c9b9dcf6eae12e760a5336 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 21 Jan 2016 08:12:27 +1100
Subject: [PATCH 27/29] mavlink: submodule update

raise version
---
 modules/mavlink | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/modules/mavlink b/modules/mavlink
index 9eb9ac0dfd..c47a4e6e1d 160000
--- a/modules/mavlink
+++ b/modules/mavlink
@@ -1 +1 @@
-Subproject commit 9eb9ac0dfda44710c4d51c932c776a3034202760
+Subproject commit c47a4e6e1d008baa7af20818906b8275df49694d

From a0af5515b89bbcdaec534575e2ea9a1485fc7ef9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 21 Jan 2016 08:12:40 +1100
Subject: [PATCH 28/29] mk: show submodule status on failure

---
 mk/check_modules.sh | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/mk/check_modules.sh b/mk/check_modules.sh
index 5edad928fc..ff25668b66 100755
--- a/mk/check_modules.sh
+++ b/mk/check_modules.sh
@@ -25,10 +25,12 @@ done
     set -x
     git submodule init || {
         echo "git submodule init failed"
+        git submodule status
         exit 1
     }
     git submodule update || {
         echo "git submodule update failed"        
+        git submodule status
         exit 1
     }
 cat <<EOF

From 68cf1b6956e8db5b731fbd1fbf8ad3bb911dcf92 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 25 Jan 2016 19:43:34 +1100
Subject: [PATCH 29/29] HAL_QURT: automatically find broadcast address for UDP
 network

---
 libraries/AP_HAL_QURT/mainapp/getifaddrs.cpp | 187 +++++++++++++++++++
 libraries/AP_HAL_QURT/mainapp/mainapp.cpp    |  12 +-
 2 files changed, 198 insertions(+), 1 deletion(-)
 create mode 100644 libraries/AP_HAL_QURT/mainapp/getifaddrs.cpp

diff --git a/libraries/AP_HAL_QURT/mainapp/getifaddrs.cpp b/libraries/AP_HAL_QURT/mainapp/getifaddrs.cpp
new file mode 100644
index 0000000000..860b7ceb30
--- /dev/null
+++ b/libraries/AP_HAL_QURT/mainapp/getifaddrs.cpp
@@ -0,0 +1,187 @@
+/* 
+   get system network addresses
+
+   based on code from Samba
+
+   Copyright (C) Andrew Tridgell 1998
+   Copyright (C) Jeremy Allison 2007
+   Copyright (C) Jelmer Vernooij <jelmer@samba.org> 2007
+
+   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 <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <ifaddrs.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+void freeifaddrs(struct ifaddrs *ifp)
+{
+	if (ifp != NULL) {
+		free(ifp->ifa_name);
+		free(ifp->ifa_addr);
+		free(ifp->ifa_netmask);
+		free(ifp->ifa_dstaddr);
+		freeifaddrs(ifp->ifa_next);
+		free(ifp);
+	}
+}
+
+static struct sockaddr *sockaddr_dup(struct sockaddr *sa)
+{
+	struct sockaddr *ret;
+	socklen_t socklen;
+	socklen = sizeof(struct sockaddr_storage);
+	ret = (struct sockaddr *)calloc(1, socklen);
+	if (ret == NULL)
+		return NULL;
+	memcpy(ret, sa, socklen);
+	return ret;
+}
+
+/* this works for Linux 2.2, Solaris 2.5, SunOS4, HPUX 10.20, OSF1
+   V4.0, Ultrix 4.4, SCO Unix 3.2, IRIX 6.4 and FreeBSD 3.2.
+
+   It probably also works on any BSD style system.  */
+
+int getifaddrs(struct ifaddrs **ifap)
+{
+	struct ifconf ifc;
+	char buff[8192];
+	int fd, i, n;
+	struct ifreq *ifr=NULL;
+	struct ifaddrs *curif;
+	struct ifaddrs *lastif = NULL;
+
+	*ifap = NULL;
+
+	if ((fd = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
+		return -1;
+	}
+  
+	ifc.ifc_len = sizeof(buff);
+	ifc.ifc_buf = buff;
+
+	if (ioctl(fd, SIOCGIFCONF, &ifc) != 0) {
+		close(fd);
+		return -1;
+	} 
+
+	ifr = ifc.ifc_req;
+  
+	n = ifc.ifc_len / sizeof(struct ifreq);
+
+	/* Loop through interfaces, looking for given IP address */
+	for (i=n-1; i>=0; i--) {
+		if (ioctl(fd, SIOCGIFFLAGS, &ifr[i]) == -1) {
+			freeifaddrs(*ifap);
+			close(fd);
+			return -1;
+		}
+
+		curif = (struct ifaddrs *)calloc(1, sizeof(struct ifaddrs));
+		if (curif == NULL) {
+			freeifaddrs(*ifap);
+			close(fd);
+			return -1;
+		}
+		curif->ifa_name = strdup(ifr[i].ifr_name);
+		if (curif->ifa_name == NULL) {
+			free(curif);
+			freeifaddrs(*ifap);
+			close(fd);
+			return -1;
+		}
+		curif->ifa_flags = ifr[i].ifr_flags;
+		curif->ifa_dstaddr = NULL;
+		curif->ifa_data = NULL;
+		curif->ifa_next = NULL;
+
+		curif->ifa_addr = NULL;
+		if (ioctl(fd, SIOCGIFADDR, &ifr[i]) != -1) {
+			curif->ifa_addr = sockaddr_dup(&ifr[i].ifr_addr);
+			if (curif->ifa_addr == NULL) {
+				free(curif->ifa_name);
+				free(curif);
+				freeifaddrs(*ifap);
+				close(fd);
+				return -1;
+			}
+		}
+
+		curif->ifa_netmask = NULL;
+		if (ioctl(fd, SIOCGIFNETMASK, &ifr[i]) != -1) {
+			curif->ifa_netmask = sockaddr_dup(&ifr[i].ifr_addr);
+			if (curif->ifa_netmask == NULL) {
+				if (curif->ifa_addr != NULL) {
+					free(curif->ifa_addr);
+				}
+				free(curif->ifa_name);
+				free(curif);
+				freeifaddrs(*ifap);
+				close(fd);
+				return -1;
+			}
+		}
+
+		if (lastif == NULL) {
+			*ifap = curif;
+		} else {
+			lastif->ifa_next = curif;
+		}
+		lastif = curif;
+	}
+
+	close(fd);
+
+	return 0;
+}  
+
+const char *get_ipv4_broadcast(void)
+{
+    struct ifaddrs *ifap = NULL;
+    if (getifaddrs(&ifap) != 0) {
+        return NULL;
+    }
+    struct ifaddrs *ia;
+    for (ia=ifap; ia; ia=ia->ifa_next) {
+        struct sockaddr_in *sin = (struct sockaddr_in *)ia->ifa_addr;
+        struct sockaddr_in *nmask = (struct sockaddr_in *)ia->ifa_netmask;
+        struct in_addr bcast;
+        if (strcmp(ia->ifa_name, "lo") == 0) {
+            continue;
+        }
+        bcast.s_addr = sin->sin_addr.s_addr | ~nmask->sin_addr.s_addr;
+        const char *ret = inet_ntoa(bcast);
+        freeifaddrs(ifap);
+        return ret;
+    }
+    freeifaddrs(ifap);
+    return NULL;
+}
+
+#ifdef MAIN_PROGRAM
+int main(void)
+{
+    printf("%s\n", get_ipv4_broadcast());
+    return 0;
+}
+#endif
diff --git a/libraries/AP_HAL_QURT/mainapp/mainapp.cpp b/libraries/AP_HAL_QURT/mainapp/mainapp.cpp
index cca09d0667..92529020c3 100644
--- a/libraries/AP_HAL_QURT/mainapp/mainapp.cpp
+++ b/libraries/AP_HAL_QURT/mainapp/mainapp.cpp
@@ -23,6 +23,8 @@ static uint64_t start_time;
 #define STORAGE_DIR "/var/APM"
 #define STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
 
+extern const char *get_ipv4_broadcast(void);
+
 // time since startup in microseconds
 static uint64_t micros64()
 {
@@ -82,6 +84,7 @@ static void get_storage(void)
  */
 static void socket_check(void)
 {
+    static const char *bcast = NULL;
     uint8_t buf[300];
     ssize_t ret = sock.recv(buf, sizeof(buf), 0);
     if (ret > 0) {
@@ -99,9 +102,16 @@ static void socket_check(void)
         }
     }
     uint32_t nbytes;
+    if (bcast == NULL) {
+        bcast = get_ipv4_broadcast();
+        if (bcast == NULL) {
+            bcast = "255.255.255.255";
+        }
+        printf("Broadcasting to %s\n", bcast);
+    }
     if (ardupilot_socket_check(buf, sizeof(buf), &nbytes) == 0) {
         if (!connected) {
-            sock.sendto(buf, nbytes, "255.255.255.255", 14550);
+            sock.sendto(buf, nbytes, bcast, 14550);
         } else {
             sock.send(buf, nbytes);
         }