Sub: Update Sub to match relevant Copter and Library developments

ArduSub/APM_Config.h

@@ -21,15 +21,15 @@
 //#define AC_FENCE              DISABLED            // disable fence to save 2k of flash
 //#define CAMERA                DISABLED            // disable camera trigger to save 1k of flash
 //#define RANGEFINDER_ENABLED   DISABLED            // disable rangefinder to save 1k of flash
+//#define PROXIMITY_ENABLED     DISABLED            // disable proximity sensors
 //#define POSHOLD_ENABLED       DISABLED            // disable PosHold flight mode to save 4.5k of flash
 //#define AC_RALLY              DISABLED            // disable rally points library (must also disable terrain which relies on rally)
 #define AC_TERRAIN            DISABLED            // disable terrain library
 #define PARACHUTE             DISABLED            // disable parachute release to save 1k of flash
-//#define EPM_ENABLED           DISABLED            // disable epm cargo gripper to save 500bytes of flash
+#define GRIPPER_ENABLED           DISABLED            // disable gripper to save 500bytes of flash
 //#define CLI_ENABLED           DISABLED            // disable the CLI (command-line-interface) to save 21K of flash space
 //#define NAV_GUIDED            DISABLED            // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
 //#define OPTFLOW               DISABLED            // disable optical flow sensor to save 5K of flash space
-//#define FRSKY_TELEM_ENABLED   DISABLED            // disable FRSky telemetry
 #define PRECISION_LANDING     DISABLED             // enable precision landing using companion computer or IRLock sensor
 #define TRANSECT_ENABLED      DISABLED
 

ArduSub/ArduSub.cpp

@@ -129,12 +129,9 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
     SCHED_TASK(rpm_update,            10,    200),
     SCHED_TASK(compass_cal_update,   100,    100),
     SCHED_TASK(accel_cal_update,      10,    100),
-#if FRSKY_TELEM_ENABLED == ENABLED
-    SCHED_TASK(frsky_telemetry_send,   5,     75),
-#endif
 	SCHED_TASK(terrain_update,        10,    100),
-#if EPM_ENABLED == ENABLED
+#if GRIPPER_ENABLED == ENABLED
-    SCHED_TASK(epm_update,            10,     75),
+    SCHED_TASK(gripper_update,            10,     75),
 #endif
 #ifdef USERHOOK_FASTLOOP
     SCHED_TASK(userhook_FastLoop,    100,     75),

ArduSub/GCS_Mavlink.cpp

@@ -338,7 +338,7 @@ void NOINLINE Sub::send_hwstatus(mavlink_channel_t chan)
     mavlink_msg_hwstatus_send(
         chan,
         hal.analogin->board_voltage()*1000,
-        hal.i2c->lockup_count());
+        0);
 }
 
 void NOINLINE Sub::send_servo_out(mavlink_channel_t chan)
@@ -392,7 +392,15 @@ void NOINLINE Sub::send_radio_out(mavlink_channel_t chan)
         hal.rcout->read(4),
         hal.rcout->read(5),
         hal.rcout->read(6),
-        hal.rcout->read(7));
+        hal.rcout->read(7),
+        hal.rcout->read(8),
+        hal.rcout->read(9),
+        hal.rcout->read(10),
+        hal.rcout->read(11),
+        hal.rcout->read(12),
+        hal.rcout->read(13),
+        hal.rcout->read(14),
+        hal.rcout->read(15));
 }
 
 void NOINLINE Sub::send_vfr_hud(mavlink_channel_t chan)
@@ -1587,20 +1595,20 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             result = sub.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4);
             break;
 
-#if EPM_ENABLED == ENABLED
+#if GRIPPER_ENABLED == ENABLED
         case MAV_CMD_DO_GRIPPER:
             // param1 : gripper number (ignored)
             // param2 : action (0=release, 1=grab). See GRIPPER_ACTIONS enum.
-            if(!sub.epm.enabled()) {
+            if(!sub.g2.gripper.enabled()) {
                 result = MAV_RESULT_FAILED;
             } else {
                 result = MAV_RESULT_ACCEPTED;
                 switch ((uint8_t)packet.param2) {
                     case GRIPPER_ACTION_RELEASE:
-                        sub.epm.release();
+                        sub.g2.gripper.release();
                         break;
                     case GRIPPER_ACTION_GRAB:
-                        sub.epm.grab();
+                        sub.g2.gripper.grab();
                         break;
                     default:
                         result = MAV_RESULT_FAILED;

ArduSub/Log.cpp

@@ -362,7 +362,7 @@ void Sub::Log_Write_Performance()
         num_loops        : perf_info_get_num_loops(),
         max_time         : perf_info_get_max_time(),
         pm_test          : pmTest1,
-        i2c_lockup_count : hal.i2c->lockup_count(),
+        i2c_lockup_count : 0,
         ins_error_count  : ins.error_count(),
 		log_dropped      : DataFlash.num_dropped() - perf_info_get_num_dropped(),
     };

ArduSub/Parameters.cpp

@@ -835,12 +835,6 @@ const AP_Param::Info Sub::var_info[] = {
     // @Path: ../libraries/AP_Relay/AP_Relay.cpp
     GOBJECT(relay,                  "RELAY_", AP_Relay),
 
-#if EPM_ENABLED == ENABLED
-	// @Group: EPM_
-    // @Path: ../libraries/AP_EPM/AP_EPM.cpp
-    GOBJECT(epm,            "EPM_", AP_EPM),
-#endif
-
 #if PARACHUTE == ENABLED
 	// @Group: CHUTE_
     // @Path: ../libraries/AP_Parachute/AP_Parachute.cpp
@@ -1072,16 +1066,37 @@ const AP_Param::Info Sub::var_info[] = {
  */
 const AP_Param::GroupInfo ParametersG2::var_info[] = {
 
-    // @Param: TKOFF_NAV_ALT
+	// @Param: WP_NAVALT_MIN
-    // @DisplayName: Takeoff navigation altitude
+	// @DisplayName: Minimum navigation altitude
-    // @Description: This is the altitude in meters above the takeoff point that attitude changes for navigation can begin
+	// @Description: This is the altitude in meters above which for navigation can begin. This applies in auto takeoff and auto landing.
 	// @Range: 0 5
 	// @User: Standard
-    AP_GROUPINFO("WP_TKOFF_NAV_ALT", 1, ParametersG2, takeoff_nav_alt, 0),
+	AP_GROUPINFO("WP_NAVALT_MIN", 1, ParametersG2, wp_navalt_min, 0),
+
+#if PROXIMITY_ENABLED == ENABLED
+    // @Group: PRX
+    // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp
+    AP_SUBGROUPINFO(proximity, "PRX", 2, ParametersG2, AP_Proximity),
+#endif
+
+#if GRIPPER_ENABLED == ENABLED
+	// @Group: GRIP_
+    // @Path: ../libraries/AP_Gripper/AP_Gripper.cpp
+    AP_SUBGROUPINFO(gripper, "GRIP_", 3, ParametersG2, AP_Gripper),
+#endif
 
     AP_GROUPEND
 };
 
+/*
+  constructor for g2 object
+ */
+ParametersG2::ParametersG2(void)
+    : proximity(sub.serial_manager)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
 /*
   This is a conversion table from old parameter values to new
   parameter names. The startup code looks for saved values of the old

ArduSub/Parameters.h

@@ -613,13 +613,23 @@ public:
 */
 class ParametersG2 {
 public:
-    ParametersG2(void) { AP_Param::setup_object_defaults(this, var_info); }
+    ParametersG2(void);
 
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo var_info[];
 
     // altitude at which nav control can start in takeoff
-    AP_Float takeoff_nav_alt;
+    AP_Float wp_navalt_min;
+
+#if GRIPPER_ENABLED
+    AP_Gripper gripper;
+#endif
+
+#if PROXIMITY_ENABLED == ENABLED
+    // proximity (aka object avoidance) library
+    AP_Proximity proximity;
+#endif
+
 };
 
 extern const AP_Param::Info        var_info[];

ArduSub/Sub.cpp

@@ -48,9 +48,6 @@ Sub::Sub(void) :
     desired_climb_rate(0),
     loiter_time_max(0),
     loiter_time(0),
-#if FRSKY_TELEM_ENABLED == ENABLED
-    frsky_telemetry(ahrs, battery),
-#endif
     climb_rate(0),
     target_rangefinder_alt(0.0f),
     baro_alt(0),
@@ -70,7 +67,7 @@ Sub::Sub(void) :
     pos_control(ahrs, inertial_nav, motors, attitude_control,
                 g.p_alt_hold, g.p_vel_z, g.pid_accel_z,
                 g.p_pos_xy, g.pi_vel_xy),
-	avoid(ahrs, inertial_nav, fence),
+	avoid(ahrs, inertial_nav, fence, g2.proximity),
     wp_nav(inertial_nav, ahrs, pos_control, attitude_control),
     circle_nav(inertial_nav, ahrs, pos_control),
     pmTest1(0),

ArduSub/Sub.h

@@ -64,6 +64,7 @@
 #include <RC_Channel/RC_Channel.h>         // RC Channel Library
 #include <AP_Motors/AP_Motors.h>          // AP Motors library
 #include <AP_RangeFinder/AP_RangeFinder.h>     // Range finder library
+#include <AP_Proximity/AP_Proximity.h>
 #include <AP_OpticalFlow/AP_OpticalFlow.h>     // Optical Flow library
 #include <AP_RSSI/AP_RSSI.h>                   // RSSI Library
 #include <Filter/Filter.h>             // Filter library
@@ -85,7 +86,6 @@
 #include <AP_Notify/AP_Notify.h>          // Notify library
 #include <AP_BattMonitor/AP_BattMonitor.h>     // Battery monitor library
 #include <AP_BoardConfig/AP_BoardConfig.h>     // board configuration library
-#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
 #include <AP_LandingGear/AP_LandingGear.h>     // Landing Gear library
 #include <AP_Terrain/AP_Terrain.h>
 #include <AP_ADSB/AP_ADSB.h>
@@ -102,8 +102,8 @@
 #if SPRAYER == ENABLED
 #include <AC_Sprayer/AC_Sprayer.h>         // crop sprayer library
 #endif
-#if EPM_ENABLED == ENABLED
+#if GRIPPER_ENABLED == ENABLED
-#include <AP_EPM/AP_EPM.h>             // EPM cargo gripper stuff
+#include <AP_Gripper/AP_Gripper.h>             // gripper stuff
 #endif
 #if PARACHUTE == ENABLED
 #include <AP_Parachute/AP_Parachute.h>       // Parachute release library
@@ -124,6 +124,7 @@ class Sub : public AP_HAL::HAL::Callbacks {
 public:
     friend class GCS_MAVLINK_Sub;
     friend class Parameters;
+    friend class ParametersG2;
 
     Sub(void);
 
@@ -413,11 +414,6 @@ private:
     // Battery Sensors
     AP_BattMonitor battery;
 
-    // FrSky telemetry support
-#if FRSKY_TELEM_ENABLED == ENABLED
-    AP_Frsky_Telem frsky_telemetry;
-#endif
-
     // Altitude
     // The cm/s we are moving up or down based on filtered data - Positive = UP
     int16_t climb_rate;
@@ -529,11 +525,6 @@ private:
     AC_Sprayer sprayer;
 #endif
 
-    // EPM Cargo Griper
-#if EPM_ENABLED == ENABLED
-    AP_EPM epm;
-#endif
-
     // Parachute release
 #if PARACHUTE == ENABLED
     AP_Parachute parachute;
@@ -949,7 +940,7 @@ private:
     void update_precland();
     void read_battery(void);
     void read_receiver_rssi(void);
-    void epm_update();
+    void gripper_update();
     void terrain_update();
     void terrain_logging();
     bool terrain_use();
@@ -987,7 +978,6 @@ private:
     bool optflow_position_ok();
     void update_auto_armed();
     void check_usb_mux(void);
-    void frsky_telemetry_send(void);
     bool should_log(uint32_t mask);
     bool current_mode_has_user_takeoff(bool must_navigate);
     bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate);
@@ -1028,7 +1018,7 @@ private:
 #if PARACHUTE == ENABLED
     void do_parachute(const AP_Mission::Mission_Command& cmd);
 #endif
-#if EPM_ENABLED == ENABLED
+#if GRIPPER_ENABLED == ENABLED
     void do_gripper(const AP_Mission::Mission_Command& cmd);
 #endif
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);

ArduSub/commands_logic.cpp

@@ -134,8 +134,8 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 #endif
 
-#if EPM_ENABLED == ENABLED
+#if GRIPPER_ENABLED == ENABLED
-    case MAV_CMD_DO_GRIPPER:                            // Mission command to control EPM gripper
+    case MAV_CMD_DO_GRIPPER:                            // Mission command to control gripper
         do_gripper(cmd);
         break;
 #endif
@@ -231,11 +231,27 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_CONDITION_YAW:
         return verify_yaw();
 
-    case MAV_CMD_DO_PARACHUTE:
+    // do commands (always return true)
-        // assume parachute was released successfully
+    case MAV_CMD_DO_CHANGE_SPEED:
+    case MAV_CMD_DO_SET_HOME:
+    case MAV_CMD_DO_SET_SERVO:
+    case MAV_CMD_DO_SET_RELAY:
+    case MAV_CMD_DO_REPEAT_SERVO:
+    case MAV_CMD_DO_REPEAT_RELAY:
+    case MAV_CMD_DO_SET_ROI:
+    case MAV_CMD_DO_MOUNT_CONTROL:
+    case MAV_CMD_DO_CONTROL_VIDEO:
+    case MAV_CMD_DO_DIGICAM_CONFIGURE:
+    case MAV_CMD_DO_DIGICAM_CONTROL:
+    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+    case MAV_CMD_DO_PARACHUTE:  // assume parachute was released successfully
+    case MAV_CMD_DO_GRIPPER:
+    case MAV_CMD_DO_GUIDED_LIMITS:
         return true;
 
     default:
+        // error message
+        gcs_send_text_fmt(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
         // return true if we do not recognize the command so that we move on to the next command
         return true;
     }
@@ -554,19 +570,19 @@ void Sub::do_parachute(const AP_Mission::Mission_Command& cmd)
 }
 #endif
 
-#if EPM_ENABLED == ENABLED
+#if GRIPPER_ENABLED == ENABLED
-// do_gripper - control EPM gripper
+// do_gripper - control gripper
 void Sub::do_gripper(const AP_Mission::Mission_Command& cmd)
 {
     // Note: we ignore the gripper num parameter because we only support one gripper
     switch (cmd.content.gripper.action) {
         case GRIPPER_ACTION_RELEASE:
-            epm.release();
+            g2.gripper.release();
-            Log_Write_Event(DATA_EPM_RELEASE);
+            Log_Write_Event(DATA_GRIPPER_RELEASE);
             break;
         case GRIPPER_ACTION_GRAB:
-            epm.grab();
+            g2.gripper.grab();
-            Log_Write_Event(DATA_EPM_GRAB);
+            Log_Write_Event(DATA_GRIPPER_GRAB);
             break;
         default:
             // do nothing

ArduSub/defines.h

@@ -55,7 +55,7 @@ enum aux_sw_func {
     AUXSW_AUTO =                16, // change to auto flight mode
     AUXSW_AUTOTUNE =            17, // auto tune
     AUXSW_LAND =                18, // change to LAND flight mode
-    AUXSW_EPM =                 19, // Operate the EPM cargo gripper low=off, middle=neutral, high=on
+    AUXSW_GRIPPER =             19, // Operate cargo grippers low=off, middle=neutral, high=on
     AUXSW_PARACHUTE_ENABLE  =   21, // Parachute enable/disable
     AUXSW_PARACHUTE_RELEASE =   22, // Parachute release
     AUXSW_PARACHUTE_3POS =      23, // Parachute disable, enable, release with 3 position switch
@@ -297,6 +297,7 @@ enum ThrowModeState {
 #define LOG_HELI_MSG                    0x20
 #define LOG_PRECLAND_MSG                0x21
 #define LOG_GUIDEDTARGET_MSG 			0x22
+#define LOG_PROXIMITY_MSG               0x24
 
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)
 #define MASK_LOG_ATTITUDE_MED           (1<<1)
@@ -352,8 +353,8 @@ enum ThrowModeState {
 #define DATA_ACRO_TRAINER_DISABLED          43
 #define DATA_ACRO_TRAINER_LEVELING          44
 #define DATA_ACRO_TRAINER_LIMITED           45
-#define DATA_EPM_GRAB                       46
+#define DATA_GRIPPER_GRAB                   46
-#define DATA_EPM_RELEASE                    47
+#define DATA_GRIPPER_RELEASE                47
 #define DATA_PARACHUTE_DISABLED             49
 #define DATA_PARACHUTE_ENABLED              50
 #define DATA_PARACHUTE_RELEASED             51

ArduSub/make.inc

@@ -44,6 +44,7 @@ LIBRARIES += AC_Circle
 LIBRARIES += AP_Declination
 LIBRARIES += AC_Fence
 LIBRARIES += AC_Avoidance
+LIBRARIES += AP_Proximity
 LIBRARIES += AP_Scheduler
 LIBRARIES += AP_RCMapper
 LIBRARIES += AP_Notify
@@ -51,7 +52,6 @@ LIBRARIES += AP_BattMonitor
 LIBRARIES += AP_BoardConfig
 LIBRARIES += AP_Frsky_Telem
 LIBRARIES += AC_Sprayer
-LIBRARIES += AP_EPM
 LIBRARIES += AP_Parachute
 LIBRARIES += AP_LandingGear
 LIBRARIES += AP_Terrain
@@ -62,3 +62,4 @@ LIBRARIES += AC_InputManager
 LIBRARIES += AP_ADSB
 LIBRARIES += AP_JSButton
 LIBRARIES += AP_LeakDetector
+LIBRARIES += AP_Gripper

ArduSub/sensors.cpp

@@ -140,7 +140,8 @@ void Sub::update_optical_flow(void)
         uint8_t flowQuality = optflow.quality();
         Vector2f flowRate = optflow.flowRate();
         Vector2f bodyRate = optflow.bodyRate();
-        ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update);
+        const Vector3f &posOffset = optflow.get_pos_offset();
+        ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update, posOffset);
         if (g.log_bitmask & MASK_LOG_OPTFLOW) {
             Log_Write_Optflow();
         }
@@ -206,10 +207,10 @@ void Sub::accel_cal_update()
     }
 }
 
-#if EPM_ENABLED == ENABLED
+#if GRIPPER_ENABLED == ENABLED
-// epm update - moves epm pwm output back to neutral after grab or release is completed
+// gripper update
-void Sub::epm_update()
+void Sub::gripper_update()
 {
-    epm.update();
+    g2.gripper.update();
 }
 #endif

ArduSub/switches.cpp

@@ -233,7 +233,7 @@ void Sub::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
         case AUXSW_RESETTOARMEDYAW:
         case AUXSW_SUPERSIMPLE_MODE:
         case AUXSW_ACRO_TRAINER:
-        case AUXSW_EPM:
+        case AUXSW_GRIPPER:
         case AUXSW_SPRAYER:
         case AUXSW_PARACHUTE_ENABLE:
         case AUXSW_PARACHUTE_3POS:      // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release
@@ -400,16 +400,16 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
                     break;
             }
             break;
-#if EPM_ENABLED == ENABLED
+#if GRIPPER_ENABLED == ENABLED
-        case AUXSW_EPM:
+        case AUXSW_GRIPPER:
             switch(ch_flag) {
                 case AUX_SWITCH_LOW:
-                    epm.release();
+                    g2.gripper.release();
-                    Log_Write_Event(DATA_EPM_RELEASE);
+                    Log_Write_Event(DATA_GRIPPER_RELEASE);
                     break;
                 case AUX_SWITCH_HIGH:
-                    epm.grab();
+                    g2.gripper.grab();
-                    Log_Write_Event(DATA_EPM_GRAB);
+                    Log_Write_Event(DATA_GRIPPER_GRAB);
                     break;
             }
             break;

ArduSub/system.cpp

@@ -111,9 +111,9 @@ void Sub::init_ardupilot()
     // initialise serial port
     serial_manager.init();
 
-    // init EPM cargo gripper
+    // init cargo gripper
-#if EPM_ENABLED == ENABLED
+#if GRIPPER_ENABLED == ENABLED
-    epm.init();
+    g2.gripper.init();
 #endif
 
     // initialise notify system
@@ -143,11 +143,6 @@ void Sub::init_ardupilot()
         gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
     }
 
-#if FRSKY_TELEM_ENABLED == ENABLED
-    // setup frsky
-    frsky_telemetry.init(serial_manager);
-#endif
-
     // identify ourselves correctly with the ground station
     mavlink_system.sysid = g.sysid_this_mav;
 
@@ -420,15 +415,6 @@ void Sub::check_usb_mux(void)
     ap.usb_connected = usb_check;
 }
 
-// frsky_telemetry_send - sends telemetry data using frsky telemetry
-//  should be called at 5Hz by scheduler
-#if FRSKY_TELEM_ENABLED == ENABLED
-void Sub::frsky_telemetry_send(void)
-{
-    frsky_telemetry.send_frames((uint8_t)control_mode);
-}
-#endif
-
 /*
   should we log a message type now?
  */

ArduSub/takeoff.cpp

@@ -114,7 +114,7 @@ void Sub::auto_takeoff_set_start_alt(void)
 
     if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
         // we are not flying, add the takeoff_nav_alt
-        auto_takeoff_no_nav_alt_cm += g2.takeoff_nav_alt * 100;
+        auto_takeoff_no_nav_alt_cm += g2.wp_navalt_min * 100;
     }
 }
 
@@ -127,7 +127,7 @@ void Sub::auto_takeoff_attitude_run(float target_yaw_rate)
 {
     float nav_roll, nav_pitch;
 
-    if (g2.takeoff_nav_alt > 0 && inertial_nav.get_altitude() < auto_takeoff_no_nav_alt_cm) {
+    if (g2.wp_navalt_min > 0 && inertial_nav.get_altitude() < auto_takeoff_no_nav_alt_cm) {
         // we haven't reached the takeoff navigation altitude yet
         nav_roll = 0;
         nav_pitch = 0;

ArduSub/wscript

@@ -17,8 +17,6 @@ def build(bld):
             'AC_Sprayer',
             'AC_WPNav',
             'AP_Camera',
-            'AP_EPM',
-            'AP_Frsky_Telem',
             'AP_IRLock',
             'AP_InertialNav',
             'AP_JSButton',
@@ -33,6 +31,8 @@ def build(bld):
             'AP_RSSI',
             'AP_Relay',
             'AP_ServoRelayEvents',
+            'AP_Proximity',
+            'AP_Gripper',
         ],
         use='mavlink',
     )