Merge branch 'ArduPilot:master' into afs-improvement2

Tools/autotest/arducopter.py

@@ -9742,7 +9742,7 @@ def ScriptMountPOI(self):
         self.setup_servo_mount()
         self.reboot_sitl()
         self.set_rc(6, 1300)
-        self.install_example_script_context('mount-poi.lua')
+        self.install_applet_script_context('mount-poi.lua')
         self.reboot_sitl()
         self.wait_ready_to_arm()
         self.context_collect('STATUSTEXT')

Tools/autotest/common.py

@@ -1986,7 +1986,8 @@ def reboot_sitl(self, required_bootcount=None, force=False):
         self.progress("Rebooting SITL")
         self.reboot_sitl_mav(required_bootcount=required_bootcount, force=force)
         self.do_heartbeats(force=True)
-        self.assert_simstate_location_is_at_startup_location()
+        if self.frame != 'sailboat':  # sailboats drift with wind!
+            self.assert_simstate_location_is_at_startup_location()
 
     def reboot_sitl_mavproxy(self, required_bootcount=None):
         """Reboot SITL instance using MAVProxy and wait for it to reconnect."""
@@ -7879,7 +7880,6 @@ def run_one_test_attempt(self, test, interact=False, attempt=1):
             passed = False
         elif not passed:  # implicit reboot after a failed test:
             self.progress("Test failed but ArduPilot process alive; rebooting")
-            self.progress("Force-rebooting SITL")
             self.reboot_sitl() # that'll learn it
 
         if self._mavproxy is not None:

Tools/scripts/build_options.py

@@ -38,7 +38,6 @@ def __init__(self,
 
     Feature('Safety', 'PARACHUTE', 'HAL_PARACHUTE_ENABLED', 'Enable Parachute', 0, None),
     Feature('Safety', 'FENCE', 'AP_FENCE_ENABLED', 'Enable Geofence', 2, None),
-    Feature('Safety', 'PROXIMITY', 'HAL_PROXIMITY_ENABLED', 'Enable Proximity', 0, None),
     Feature('Safety', 'AC_AVOID', 'AC_AVOID_ENABLED', 'Enable Avoidance', 0, 'FENCE'),
     Feature('Safety', 'AC_OAPATHPLANNER', 'AC_OAPATHPLANNER_ENABLED', 'Enable Object Avoidance Path Planner', 0, 'FENCE'),
 
@@ -218,6 +217,17 @@ def __init__(self,
     Feature('Sensors', 'OPTICALFLOW_PIXART', 'AP_OPTICALFLOW_PIXART_ENABLED', 'Enable Optical flow PIXART Sensor', 0, "OPTICALFLOW"),   # NOQA: E501
     Feature('Sensors', 'OPTICALFLOW_UPFLOW', 'AP_OPTICALFLOW_UPFLOW_ENABLED', 'Enable Optical flow UPFLOW Sensor', 0, "OPTICALFLOW"),   # NOQA: E501
 
+    Feature('Proximity', 'PROXIMITY', 'HAL_PROXIMITY_ENABLED', 'Enable Proximity', 0, None),
+    Feature('Proximity', 'PROXIMITY_CYGBOT', 'AP_PROXIMITY_CYGBOT_ENABLED', 'Enable Cygbot D1 Proximity Sensors', 0, "PROXIMITY"),  # noqa
+    Feature('Proximity', 'PROXIMITY_DRONECAN', 'AP_PROXIMITY_DRONECAN_ENABLED', 'Enable DroneCAN Proximity Sensors', 0, "PROXIMITY"),  # noqa
+    Feature('Proximity', 'PROXIMITY_LIGHTWARE_SF40C', 'AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED', 'Enable LightWare SF40C Proximity Sensors', 0, "PROXIMITY"),  # noqa
+    Feature('Proximity', 'PROXIMITY_LIGHTWARE_SF45B', 'AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED', 'Enable LightWare SF45B Proximity Sensors', 0, "PROXIMITY"),  # noqa
+    Feature('Proximity', 'PROXIMITY_MAV', 'AP_PROXIMITY_MAV_ENABLED', 'Enable MAVLink Proximity Sensors', 0, "PROXIMITY"),  # noqa
+    Feature('Proximity', 'PROXIMITY_RANGEFINDER', 'AP_PROXIMITY_RANGEFINDER_ENABLED', 'Use RangeFinders as proximity sensors', 0, "PROXIMITY,RANGEFINDER"),  # noqa
+    Feature('Proximity', 'PROXIMITY_RPLIDARA2', 'AP_PROXIMITY_RPLIDARA2_ENABLED', 'Enable RPLidarA2 Proximity Sensors', 0, "PROXIMITY"),  # noqa
+    Feature('Proximity', 'PROXIMITY_TERRARANGERTOWER', 'AP_PROXIMITY_TERARANGERTOWER_ENABLED', 'Enable TerraRangerTower Proximity Sensors', 0, "PROXIMITY"),  # noqa
+    Feature('Proximity', 'PROXIMITY_TERRARANGERTOWEREVO', 'AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED', 'Enable TerraRangerTower Evo Proximity Sensors', 0, "PROXIMITY"),  # noqa
+
     Feature('Sensors', 'BMP085', 'AP_BARO_BMP085_ENABLED', 'Enable BMP085 Barometric Sensor', 1, None),
     Feature('Sensors', 'BMP280', 'AP_BARO_BMP280_ENABLED', 'Enable BMP280 Barometric Sensor', 1, None),
     Feature('Sensors', 'BMP388', 'AP_BARO_BMP388_ENABLED', 'Enable BMP388 Barometric Sensor', 1, None),

Tools/scripts/extract_features.py

@@ -116,9 +116,13 @@ def __init__(self, filename, nm="arm-none-eabi-nm"):
             ('AP_CAMERA_{type}_ENABLED', 'AP_Camera_(?P<type>.*)::trigger_pic',),
             ('HAL_RUNCAM_ENABLED', 'AP_RunCam::AP_RunCam',),
 
+            ('HAL_PROXIMITY_ENABLED', 'AP_Proximity::AP_Proximity',),
+            ('AP_PROXIMITY_{type}_ENABLED', 'AP_Proximity_(?P<type>.*)::update',),
+            ('AP_PROXIMITY_CYGBOT_ENABLED', 'AP_Proximity_Cygbot_D1::update',),
+            ('AP_PROXIMITY_LIGHTWARE_{type}_ENABLED', 'AP_Proximity_LightWare(?P<type>.*)::update',),
+
             ('HAL_PARACHUTE_ENABLED', 'AP_Parachute::update',),
             ('AP_FENCE_ENABLED', r'AC_Fence::check\b',),
-            ('HAL_PROXIMITY_ENABLED', 'AP_Proximity::AP_Proximity',),
             ('AC_AVOID_ENABLED', 'AC_Avoid::AC_Avoid',),
             ('AC_OAPATHPLANNER_ENABLED', 'AP_OAPathPlanner::AP_OAPathPlanner',),
 
@@ -174,7 +178,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm"):
 
             ('AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED', r'RC_Channel::lookuptable',),
 
-            ('AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED', r'AP_Notify::play_tune'),
+            ('AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED', r'AP_Notify::handle_play_tune'),
             ('AP_NOTIFY_MAVLINK_LED_CONTROL_SUPPORT_ENABLED', r'AP_Notify::handle_led_control'),
             ('AP_NOTIFY_NCP5623_ENABLED', r'NCP5623::write'),
             ('AP_NOTIFY_PROFILED_ENABLED', r'ProfiLED::init_ports'),

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -305,21 +305,21 @@ AP_BattMonitor::init()
                 drivers[instance] = new AP_BattMonitor_SMBus_NeoDesign(*this, state[instance], _params[instance]);
                 break;
 #endif
-            case Type::BEBOP:
 #if AP_BATTERY_BEBOP_ENABLED
+            case Type::BEBOP:
                 drivers[instance] = new AP_BattMonitor_Bebop(*this, state[instance], _params[instance]);
-#endif
                 break;
-            case Type::UAVCAN_BatteryInfo:
+#endif
 #if AP_BATTERY_UAVCAN_BATTERYINFO_ENABLED
+            case Type::UAVCAN_BatteryInfo:
                 drivers[instance] = new AP_BattMonitor_DroneCAN(*this, state[instance], AP_BattMonitor_DroneCAN::UAVCAN_BATTERY_INFO, _params[instance]);
-#endif
                 break;
-            case Type::BLHeliESC:
+#endif
 #if AP_BATTERY_ESC_ENABLED
+            case Type::BLHeliESC:
                 drivers[instance] = new AP_BattMonitor_ESC(*this, state[instance], _params[instance]);
-#endif
                 break;
+#endif
 #if AP_BATTERY_SUM_ENABLED
             case Type::Sum:
                 drivers[instance] = new AP_BattMonitor_Sum(*this, state[instance], _params[instance], instance);

libraries/AP_Camera/AP_Camera.cpp

@@ -17,6 +17,7 @@
 #include "AP_Camera_Mount.h"
 #include "AP_Camera_MAVLink.h"
 #include "AP_Camera_MAVLinkCamV2.h"
+#include "AP_Camera_Scripting.h"
 
 const AP_Param::GroupInfo AP_Camera::var_info[] = {
 
@@ -60,6 +61,8 @@ AP_Camera::AP_Camera(uint32_t _log_camera_bit) :
 // set camera trigger distance in a mission
 void AP_Camera::set_trigger_distance(float distance_m)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return;
     }
@@ -69,6 +72,8 @@ void AP_Camera::set_trigger_distance(float distance_m)
 // momentary switch to change camera between picture and video modes
 void AP_Camera::cam_mode_toggle()
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return;
     }
@@ -78,6 +83,8 @@ void AP_Camera::cam_mode_toggle()
 // take a picture
 void AP_Camera::take_picture()
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return;
     }
@@ -88,6 +95,8 @@ void AP_Camera::take_picture()
 // start_recording should be true to start recording, false to stop recording
 bool AP_Camera::record_video(bool start_recording)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return false;
     }
@@ -98,6 +107,8 @@ bool AP_Camera::record_video(bool start_recording)
 // zoom out = -1, hold = 0, zoom in = 1
 bool AP_Camera::set_zoom_step(int8_t zoom_step)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return false;
     }
@@ -108,6 +119,8 @@ bool AP_Camera::set_zoom_step(int8_t zoom_step)
 // focus in = -1, focus hold = 0, focus out = 1
 bool AP_Camera::set_manual_focus_step(int8_t focus_step)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return false;
     }
@@ -117,6 +130,8 @@ bool AP_Camera::set_manual_focus_step(int8_t focus_step)
 // auto focus
 bool AP_Camera::set_auto_focus()
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return false;
     }
@@ -170,6 +185,12 @@ void AP_Camera::init()
         case CameraType::MAVLINK_CAMV2:
             _backends[instance] = new AP_Camera_MAVLinkCamV2(*this, _params[instance], instance);
             break;
+#endif
+#if AP_CAMERA_SCRIPTING_ENABLED
+        // check for Scripting driver
+        case CameraType::SCRIPTING:
+            _backends[instance] = new AP_Camera_Scripting(*this, _params[instance], instance);
+            break;
 #endif
         case CameraType::NONE:
             break;
@@ -192,6 +213,8 @@ void AP_Camera::init()
 // handle incoming mavlink messages
 void AP_Camera::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (msg.msgid == MAVLINK_MSG_ID_DIGICAM_CONTROL) {
         // decode deprecated MavLink message that controls camera.
         __mavlink_digicam_control_t packet;
@@ -280,6 +303,8 @@ MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet)
 // set camera trigger distance in a mission
 void AP_Camera::set_trigger_distance(uint8_t instance, float distance_m)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return;
@@ -292,6 +317,8 @@ void AP_Camera::set_trigger_distance(uint8_t instance, float distance_m)
 // momentary switch to change camera between picture and video modes
 void AP_Camera::cam_mode_toggle(uint8_t instance)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return;
@@ -304,6 +331,8 @@ void AP_Camera::cam_mode_toggle(uint8_t instance)
 // configure camera
 void AP_Camera::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return;
     }
@@ -312,6 +341,8 @@ void AP_Camera::configure(float shooting_mode, float shutter_speed, float apertu
 
 void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return;
@@ -324,6 +355,8 @@ void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_s
 // handle camera control
 void AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return;
     }
@@ -332,6 +365,8 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo
 
 void AP_Camera::control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return;
@@ -344,8 +379,10 @@ void AP_Camera::control(uint8_t instance, float session, float zoom_pos, float z
 /*
   Send camera feedback to the GCS
  */
-void AP_Camera::send_feedback(mavlink_channel_t chan) const
+void AP_Camera::send_feedback(mavlink_channel_t chan)
 {
+    WITH_SEMAPHORE(_rsem);
+
     // call each instance
     for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
         if (_backends[instance] != nullptr) {
@@ -359,6 +396,8 @@ void AP_Camera::send_feedback(mavlink_channel_t chan) const
 */
 void AP_Camera::update()
 {
+    WITH_SEMAPHORE(_rsem);
+
     // call each instance
     for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
         if (_backends[instance] != nullptr) {
@@ -370,6 +409,8 @@ void AP_Camera::update()
 // take_picture - take a picture
 void AP_Camera::take_picture(uint8_t instance)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return;
@@ -383,6 +424,8 @@ void AP_Camera::take_picture(uint8_t instance)
 // start_recording should be true to start recording, false to stop recording
 bool AP_Camera::record_video(uint8_t instance, bool start_recording)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;
@@ -396,6 +439,8 @@ bool AP_Camera::record_video(uint8_t instance, bool start_recording)
 // zoom out = -1, hold = 0, zoom in = 1
 bool AP_Camera::set_zoom_step(uint8_t instance, int8_t zoom_step)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;
@@ -409,6 +454,8 @@ bool AP_Camera::set_zoom_step(uint8_t instance, int8_t zoom_step)
 // focus in = -1, focus hold = 0, focus out = 1
 bool AP_Camera::set_manual_focus_step(uint8_t instance, int8_t focus_step)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;
@@ -421,6 +468,8 @@ bool AP_Camera::set_manual_focus_step(uint8_t instance, int8_t focus_step)
 // auto focus.  returns true on success
 bool AP_Camera::set_auto_focus(uint8_t instance)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;
@@ -430,8 +479,23 @@ bool AP_Camera::set_auto_focus(uint8_t instance)
     return backend->set_auto_focus();
 }
 
+#if AP_CAMERA_SCRIPTING_ENABLED
+// accessor to allow scripting backend to retrieve state
+// returns true on success and cam_state is filled in
+bool AP_Camera::get_state(uint8_t instance, camera_state_t& cam_state)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return false;
+    }
+    return backend->get_state(cam_state);
+}
+#endif // #if AP_CAMERA_SCRIPTING_ENABLED
+
 // return backend for instance number
-AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance)
+AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance) const
 {
     if (instance >= ARRAY_SIZE(_backends)) {
         return nullptr;