diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 20e85965af..ede14771a4 100644 --- a/ArduSub/ArduSub.cpp +++ b/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() ); diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 3f1ac6a0fd..734fdb3a73 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -1847,10 +1847,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; #if PRECISION_LANDING == ENABLED - case MAVLINK_MSG_ID_LANDING_TARGET: - // configure or release parachute - result = MAV_RESULT_ACCEPTED; - sub.precland.handle_msg(msg); + case MAVLINK_MSG_ID_LANDING_TARGET: + // configure or release parachute + result = MAV_RESULT_ACCEPTED; + copter.precland.handle_msg(msg); + break; #endif #if CAMERA == ENABLED diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index f9d299544e..f06a717ff2 100644 --- a/ArduSub/Log.cpp +++ b/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) {} diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 7bb5008020..f3104ea1a6 100644 --- a/ArduSub/Parameters.cpp +++ b/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_ diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index a00351e17d..fd54132fa0 100644 --- a/ArduSub/Parameters.h +++ b/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 diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index fb70da0402..b89e035bc2 100644 --- a/ArduSub/Sub.h +++ b/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); diff --git a/ArduSub/control_autotune.cpp b/ArduSub/control_autotune.cpp index 13777c850c..c36deecf26 100644 --- a/ArduSub/control_autotune.cpp +++ b/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: diff --git a/ArduSub/control_land.cpp b/ArduSub/control_land.cpp index 22378d835a..1e15ab7080 100644 --- a/ArduSub/control_land.cpp +++ b/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); diff --git a/ArduSub/control_throw.cpp b/ArduSub/control_throw.cpp index e32b7fe37e..31872ca4c3 100644 --- a/ArduSub/control_throw.cpp +++ b/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; - } + return true; } // clean up when exiting throw mode diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 328cdcf5e5..510fe218fc 100644 --- a/ArduSub/defines.h +++ b/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 diff --git a/ArduSub/wscript b/ArduSub/wscript index 68244067c7..12ed2990de 100644 --- a/ArduSub/wscript +++ b/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', ) - bld.ap_program( - program_name='ardusub', - use=vehicle + '_libs', + frames = ( + 'bluerov','vectored','vectored6DOF', ) + + for frame in frames: + frame_config = frame.upper().replace('-','_') + '_FRAME' + bld.ap_program( + program_name='ardusub-%s' % frame, + program_groups=['bin', 'sub'], + use=vehicle + '_libs', + defines=['FRAME_CONFIG=%s' % frame_config], + )