Sub: Match Copter changes

ArduSub/ArduSub.cpp

@@ -386,7 +386,7 @@ void Sub::ten_hz_logging_loop()
     // log attitude data if we're not already logging at the higher rate
     if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
         Log_Write_Attitude();
-        Log_Write_Rate();
+        DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
         if (should_log(MASK_LOG_PID)) {
             DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
             DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );
@@ -429,7 +429,7 @@ void Sub::fifty_hz_logging_loop()
 #if HIL_MODE == HIL_MODE_DISABLED
     if (should_log(MASK_LOG_ATTITUDE_FAST)) {
         Log_Write_Attitude();
-        Log_Write_Rate();
+        DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
         if (should_log(MASK_LOG_PID)) {
             DataFlash.Log_Write_PID(LOG_PIDR_MSG, g.pid_rate_roll.get_pid_info() );
             DataFlash.Log_Write_PID(LOG_PIDP_MSG, g.pid_rate_pitch.get_pid_info() );

ArduSub/GCS_Mavlink.cpp

@@ -1850,7 +1850,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
     case MAVLINK_MSG_ID_LANDING_TARGET:
         // configure or release parachute
         result = MAV_RESULT_ACCEPTED;
-            sub.precland.handle_msg(msg);
+        copter.precland.handle_msg(msg);
+        break;
 #endif
 
 #if CAMERA == ENABLED

ArduSub/Log.cpp

@@ -376,47 +376,6 @@ void Sub::Log_Write_Attitude()
     DataFlash.Log_Write_POS(ahrs);
 }
 
-struct PACKED log_Rate {
-    LOG_PACKET_HEADER;
-    uint64_t time_us;
-    float   control_roll;
-    float   roll;
-    float   roll_out;
-    float   control_pitch;
-    float   pitch;
-    float   pitch_out;
-    float   control_yaw;
-    float   yaw;
-    float   yaw_out;
-    float   control_accel;
-    float   accel;
-    float   accel_out;
-};
-
-// Write an rate packet
-void Sub::Log_Write_Rate()
-{
-    const Vector3f &rate_targets = attitude_control.rate_bf_targets();
-    const Vector3f &accel_target = pos_control.get_accel_target();
-    struct log_Rate pkt_rate = {
-        LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
-        time_us         : AP_HAL::micros64(),
-        control_roll    : (float)rate_targets.x,
-        roll            : (float)(ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100),
-        roll_out        : motors.get_roll(),
-        control_pitch   : (float)rate_targets.y,
-        pitch           : (float)(ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100),
-        pitch_out       : motors.get_pitch(),
-        control_yaw     : (float)rate_targets.z,
-        yaw             : (float)(ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100),
-        yaw_out         : motors.get_yaw(),
-        control_accel   : (float)accel_target.z,
-        accel           : (float)(-(ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f),
-        accel_out       : motors.get_throttle()
-    };
-    DataFlash.WriteBlock(&pkt_rate, sizeof(pkt_rate));
-}
-
 struct PACKED log_MotBatt {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -727,8 +686,6 @@ const struct LogStructure Sub::log_structure[] = {
       "CTUN", "Qhhfffecchh", "TimeUS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
       "PM",  "QHHIhBH",    "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr" },
-    { LOG_RATE_MSG, sizeof(log_Rate),
-      "RATE", "Qffffffffffff",  "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut" },
     { LOG_MOTBATT_MSG, sizeof(log_MotBatt),
       "MOTB", "Qffff",  "TimeUS,LiftMax,BatVolt,BatRes,ThLimit" },
     { LOG_STARTUP_MSG, sizeof(log_Startup),         
@@ -830,7 +787,6 @@ void Sub::Log_Write_Nav_Tuning() {}
 void Sub::Log_Write_Control_Tuning() {}
 void Sub::Log_Write_Performance() {}
 void Sub::Log_Write_Attitude(void) {}
-void Sub::Log_Write_Rate() {}
 void Sub::Log_Write_MotBatt() {}
 void Sub::Log_Write_Startup() {}
 void Sub::Log_Write_Event(uint8_t id) {}

ArduSub/Parameters.cpp

@@ -249,6 +249,15 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Standard
     GSCALAR(land_speed,             "LAND_SPEED",   LAND_SPEED),
 
+    // @Param: LAND_SPEED_HIGH
+    // @DisplayName: Land speed high
+    // @Description: The descent speed for the first stage of landing in cm/s. If this is zero then WPNAV_SPEED_DN is used
+    // @Units: cm/s
+    // @Range: 0 500
+    // @Increment: 10
+    // @User: Standard
+    GSCALAR(land_speed_high,        "LAND_SPEED_HIGH",   0),    
+
     // @Param: PILOT_VELZ_MAX
     // @DisplayName: Pilot maximum vertical speed
     // @Description: The maximum vertical velocity the pilot may request in cm/s
@@ -938,7 +947,7 @@ const AP_Param::Info Sub::var_info[] = {
     GOBJECT(landinggear,    "LGR_", AP_LandingGear),
 
     // @Group: COMPASS_
-    // @Path: ../libraries/AP_Compass/Compass.cpp
+    // @Path: ../libraries/AP_Compass/AP_Compass.cpp
     GOBJECT(compass,        "COMPASS_", Compass),
 
     // @Group: INS_

ArduSub/Parameters.h

@@ -321,6 +321,7 @@ public:
         k_param_land_speed,
         k_param_auto_velocity_z_min, // remove
         k_param_auto_velocity_z_max, // remove - 219
+        k_param_land_speed_high,
 
         //
         // 220: PI/D Controllers
@@ -426,6 +427,7 @@ public:
     //
     AP_Int32        rtl_loiter_time;
     AP_Int16        land_speed;
+    AP_Int16        land_speed_high;
     AP_Int16        pilot_velocity_z_max;        // maximum vertical velocity the pilot may request
     AP_Int16        pilot_accel_z;               // vertical acceleration the pilot may request
 

ArduSub/Sub.h

@@ -624,7 +624,6 @@ private:
     void Log_Write_Control_Tuning();
     void Log_Write_Performance();
     void Log_Write_Attitude();
-    void Log_Write_Rate();
     void Log_Write_MotBatt();
     void Log_Write_Startup();
     void Log_Write_Event(uint8_t id);

ArduSub/control_autotune.cpp

@@ -516,7 +516,7 @@ void Copter::autotune_attitude_control()
 
         // log this iterations lean angle and rotation rate
         Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
-        Log_Write_Rate();
+        DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
         break;
 
     case AUTOTUNE_STEP_UPDATE_GAINS:

ArduSub/control_land.cpp

@@ -207,6 +207,10 @@ float Sub::get_land_descent_speed()
 #endif
     // if we are above 10m and the sonar does not sense anything perform regular alt hold descent
     if (pos_control.get_pos_target().z <= pv_alt_above_origin(LAND_START_ALT) && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
+        if (g.land_speed_high > 0) {
+            // user has asked for a different landing speed than normal descent rate
+            return -abs(g.land_speed_high);
+        }
         return pos_control.get_speed_up();
     }else{
         return abs(g.land_speed);

ArduSub/control_throw.cpp

@@ -17,11 +17,7 @@ bool Sub::throw_init(bool ignore_checks)
     }
 
     // this mode needs a position reference
-    if (position_ok()) {
     return true;
-    } else {
-        return false;
-    }
 }
 
 // clean up when exiting throw mode

ArduSub/defines.h

@@ -268,7 +268,6 @@ enum ThrowModeState {
 #define LOG_DATA_FLOAT_MSG              0x18
 #define LOG_AUTOTUNE_MSG                0x19
 #define LOG_AUTOTUNEDETAILS_MSG         0x1A
-#define LOG_RATE_MSG                    0x1D
 #define LOG_MOTBATT_MSG                 0x1E
 #define LOG_PARAMTUNE_MSG               0x1F
 #define LOG_HELI_MSG                    0x20

ArduSub/wscript

@@ -8,6 +8,7 @@ def build(bld):
         vehicle=vehicle,
         libraries=bld.ap_common_vehicle_libraries() + [
             'AC_AttitudeControl',
+            'AC_InputManager',
             'AC_Fence',
             'AC_PID',
             'AC_PrecLand',
@@ -33,7 +34,15 @@ def build(bld):
         use='mavlink',
     )
 
+    frames = (
+        'bluerov','vectored','vectored6DOF',
+    )
+
+    for frame in frames:
+        frame_config = frame.upper().replace('-','_') + '_FRAME'
         bld.ap_program(
-        program_name='ardusub',
+            program_name='ardusub-%s' % frame,
+            program_groups=['bin', 'sub'],
             use=vehicle + '_libs',
+            defines=['FRAME_CONFIG=%s' % frame_config],
         )