From 814605c461d2ddb51c108878f5fee3faa6548a68 Mon Sep 17 00:00:00 2001 From: Rustom Jehangir Date: Tue, 5 Jul 2016 20:18:58 -0400 Subject: [PATCH] Sub: Make changes to match Copter changes --- ArduSub/GCS_Mavlink.cpp | 28 ++++++++++++++++++++++++++++ ArduSub/Log.cpp | 10 ++++++---- ArduSub/Parameters.cpp | 35 +++++++++++++++-------------------- ArduSub/Parameters.h | 7 +++---- ArduSub/Sub.cpp | 4 ++-- ArduSub/Sub.h | 7 ++++++- ArduSub/capabilities.cpp | 4 +++- ArduSub/config.h | 8 -------- ArduSub/control_auto.cpp | 16 ++++++++-------- ArduSub/control_brake.cpp | 4 ++-- ArduSub/control_circle.cpp | 4 ++-- ArduSub/control_drift.cpp | 2 +- ArduSub/control_flip.cpp | 2 +- ArduSub/control_guided.cpp | 32 +++++++++++++++----------------- ArduSub/control_land.cpp | 4 ++-- ArduSub/control_rtl.cpp | 12 ++++++------ ArduSub/control_stabilize.cpp | 2 +- ArduSub/control_throw.cpp | 6 +++--- ArduSub/defines.h | 9 +++++---- ArduSub/land_detector.cpp | 5 +++++ ArduSub/make.inc | 2 ++ ArduSub/system.cpp | 2 ++ ArduSub/wscript | 2 ++ 23 files changed, 120 insertions(+), 87 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 153acb04c8..07614e686e 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -744,6 +744,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) case MSG_MAG_CAL_REPORT: sub.compass.send_mag_cal_report(chan); break; + + case MSG_ADSB_VEHICLE: + CHECK_PAYLOAD_SIZE(ADSB_VEHICLE); + sub.adsb.send_adsb_vehicle(chan); + break; } return true; @@ -831,6 +836,15 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Increment: 1 // @User: Advanced AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 0), + + // @Param: ADSB + // @DisplayName: ADSB stream rate to ground station + // @Description: ADSB stream rate to ground station + // @Units: Hz + // @Range: 0 50 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("ADSB", 9, GCS_MAVLINK, streamRates[9], 5), AP_GROUPEND }; @@ -971,6 +985,12 @@ GCS_MAVLINK::data_stream_send(void) send_message(MSG_VIBRATION); send_message(MSG_RPM); } + + if (sub.gcs_out_of_time) return; + + if (stream_trigger(STREAM_ADSB)) { + send_message(MSG_ADSB_VEHICLE); + } } @@ -1948,6 +1968,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; #endif +#if AC_FENCE == ENABLED + // send or receive fence points with GCS + case MAVLINK_MSG_ID_FENCE_POINT: // MAV ID: 160 + case MAVLINK_MSG_ID_FENCE_FETCH_POINT: + sub.fence.handle_msg(chan, msg); + break; +#endif // AC_FENCE == ENABLED + #if CAMERA == ENABLED //deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: // MAV ID: 202 diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 89213a0b69..f10ae7d5d0 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -301,13 +301,14 @@ struct PACKED log_Control_Tuning { float throttle_in; float angle_boost; float throttle_out; + float throttle_hover; float desired_alt; float inav_alt; int32_t baro_alt; int16_t desired_rangefinder_alt; int16_t rangefinder_alt; float terr_alt; - int16_t desired_climb_rate; + int16_t target_climb_rate; int16_t climb_rate; }; @@ -326,13 +327,14 @@ void Sub::Log_Write_Control_Tuning() throttle_in : attitude_control.get_throttle_in(), angle_boost : attitude_control.angle_boost(), throttle_out : motors.get_throttle(), + throttle_hover : motors.get_throttle_hover(), desired_alt : pos_control.get_alt_target() / 100.0f, inav_alt : inertial_nav.get_altitude() / 100.0f, baro_alt : baro_alt, desired_rangefinder_alt : (int16_t)target_rangefinder_alt, rangefinder_alt : rangefinder_state.alt_cm, terr_alt : terr_alt, - desired_climb_rate : (int16_t)pos_control.get_vel_target_z(), + target_climb_rate : (int16_t)pos_control.get_vel_target_z(), climb_rate : climb_rate }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -708,7 +710,7 @@ const struct LogStructure Sub::log_structure[] = { { LOG_NAV_TUNING_MSG, sizeof(log_Nav_Tuning), "NTUN", "Qffffffffff", "TimeUS,DPosX,DPosY,PosX,PosY,DVelX,DVelY,VelX,VelY,DAccX,DAccY" }, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), - "CTUN", "Qfffffeccfhh", "TimeUS,ThrIn,ABst,ThrOut,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt" }, + "CTUN", "Qffffffeccfhh", "TimeUS,ThrIn,ABst,ThrOut,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt" }, { LOG_PERFORMANCE_MSG, sizeof(log_Performance), "PM", "QHHIhBHI", "TimeUS,NLon,NLoop,MaxT,PMT,I2CErr,INSErr,LogDrop" }, { LOG_MOTBATT_MSG, sizeof(log_MotBatt), @@ -825,7 +827,7 @@ void Sub::Log_Write_Baro(void) {} void Sub::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {} void Sub::Log_Write_Home_And_Origin() {} void Sub::Log_Sensor_Health() {} -void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}; +void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} #if OPTFLOW == ENABLED void Sub::Log_Write_Optflow() {} diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 05b42f125a..5f815226fc 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -286,15 +286,6 @@ const AP_Param::Info Sub::var_info[] = { // @User: Standard GSCALAR(pilot_accel_z, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT), - // @Param: THR_MIN - // @DisplayName: Throttle Minimum - // @Description: The minimum throttle that will be sent to the motors to keep them spinning - // @Units: Percent*10 - // @Range: 0 300 - // @Increment: 1 - // @User: Standard - GSCALAR(throttle_min, "THR_MIN", THR_MIN_DEFAULT), - // @Param: FS_THR_ENABLE // @DisplayName: Throttle Failsafe Enable // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel @@ -311,15 +302,6 @@ const AP_Param::Info Sub::var_info[] = { // @User: Standard GSCALAR(failsafe_throttle_value, "FS_THR_VALUE", FS_THR_VALUE_DEFAULT), - // @Param: THR_MID - // @DisplayName: Throttle Mid Position - // @Description: The throttle output (0 ~ 1000) when throttle stick is in mid position. Used to scale the manual throttle so that the mid throttle stick position is close to the throttle required to hover - // @User: Standard - // @Range: 300 700 - // @Units: Percent*10 - // @Increment: 10 - GSCALAR(throttle_mid, "THR_MID", THR_MID_DEFAULT), - // @Param: THR_DZ // @DisplayName: Throttle deadzone // @Description: The deadzone above and below mid throttle. Used in AltHold, Loiter, PosHold flight modes @@ -863,7 +845,7 @@ const AP_Param::Info Sub::var_info[] = { GOBJECT(circle_nav, "CIRCLE_", AC_Circle), // @Group: ATC_ - // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp + // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp GOBJECT(attitude_control, "ATC_", AC_AttitudeControl_Multi), // @Group: PSC @@ -937,6 +919,10 @@ const AP_Param::Info Sub::var_info[] = { GOBJECT(fence, "FENCE_", AC_Fence), #endif + // @Group: AVOID_ + // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp + GOBJECT(avoid, "AVOID_", AC_Avoid), + #if AC_RALLY == ENABLED // @Group: RALLY_ // @Path: ../libraries/AP_Rally/AP_Rally.cpp @@ -1042,7 +1028,7 @@ const AP_Param::Info Sub::var_info[] = { // @Param: TERRAIN_FOLLOW // @DisplayName: Terrain Following use control // @Description: This enables terrain following for RTL and LAND flight modes. To use this option TERRAIN_ENABLE must be 1 and the GCS must support sending terrain data to the aircraft. In RTL the RTL_ALT will be considered a height above the terrain. In LAND mode the vehicle will slow to LAND_SPEED 10m above terrain (instead of 10m above home). This parameter does not affect AUTO and Guided which use a per-command flag to determine if the height is above-home, absolute or above-terrain. - // @Values: 0:Do Not Use in RTL and Land 1:Use in RTL and Land + // @Values: 0:Do Not Use in RTL and Land,1:Use in RTL and Land // @User: Standard GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0), @@ -1136,6 +1122,10 @@ void Sub::convert_pid_parameters(void) { Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" }, { Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" } }; + AP_Param::ConversionInfo throttle_conversion_info[] = { + { Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" }, + { Parameters::k_param_throttle_mid, 0, AP_PARAM_FLOAT, "MOT_THST_HOVER" } + }; // gains increase by 27% due to attitude controller's switch to use radians instead of centi-degrees // and motor libraries switch to accept inputs in -1 to +1 range instead of -4500 ~ +4500 @@ -1161,4 +1151,9 @@ void Sub::convert_pid_parameters(void) for (uint8_t i=0; i // circle navigation library #include // ArduPilot Mega Declination Helper Library #include // ArduCopter Fence library +#include // Arducopter stop at fence library #include // main loop scheduler #include // RC input mapping library #include // Notify library @@ -87,6 +88,7 @@ #include #include // Landing Gear library #include +#include #include #include // Pilot input handling library #include // Joystick/gamepad button function assignment @@ -476,6 +478,7 @@ private: AC_AttitudeControl_Multi attitude_control; AC_PosControl pos_control; + AC_Avoid avoid; AC_WPNav wp_nav; AC_Circle circle_nav; @@ -552,6 +555,8 @@ private: AC_PrecLand precland; #endif + AP_ADSB adsb {ahrs}; + // use this to prevent recursion during sensor init bool in_mavlink_delay; @@ -617,7 +622,7 @@ private: void check_ekf_yaw_reset(); float get_roi_yaw(); float get_look_ahead_yaw(); - void update_thr_average(); + void update_throttle_hover(); void set_throttle_takeoff(); float get_pilot_desired_throttle(int16_t throttle_control); float get_pilot_desired_climb_rate(float throttle_control); diff --git a/ArduSub/capabilities.cpp b/ArduSub/capabilities.cpp index 1b2e1b590a..dd5ac9aec8 100644 --- a/ArduSub/capabilities.cpp +++ b/ArduSub/capabilities.cpp @@ -11,5 +11,7 @@ void Sub::init_capabilities(void) hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT); hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION); hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET); - hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN); +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN); +#endif } diff --git a/ArduSub/config.h b/ArduSub/config.h index 0ce335f14b..c1afa36cd4 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -509,14 +509,6 @@ ////////////////////////////////////////////////////////////////////////////// // Throttle control gains // -#ifndef THR_MID_DEFAULT - # define THR_MID_DEFAULT 500 // Throttle output (0 ~ 1000) when throttle stick is in mid position -#endif - -#ifndef THR_MIN_DEFAULT - # define THR_MIN_DEFAULT 0 // minimum throttle sent to the motors when armed and pilot throttle above zero -#endif -#define THR_MAX 1000 // maximum throttle input and output sent to the motors #ifndef THR_DZ_DEFAULT # define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index e7926a3f20..b725e604fe 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -174,7 +174,7 @@ void Sub::auto_takeoff_run() pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination @@ -251,10 +251,10 @@ void Sub::auto_wp_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true); + attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain()); } } @@ -319,10 +319,10 @@ void Sub::auto_spline_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true); + attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain()); } } @@ -416,7 +416,7 @@ void Sub::auto_land_run() desired_climb_rate = cmb_rate; // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); } // auto_rtl_start - initialises RTL in AUTO flight mode @@ -512,7 +512,7 @@ void Sub::auto_circle_run() pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true); + attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain()); } #if NAV_GUIDED == ENABLED @@ -589,7 +589,7 @@ void Sub::auto_loiter_run() failsafe_terrain_set_status(wp_nav.update_wpnav()); pos_control.update_z_controller(); - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); } // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter diff --git a/ArduSub/control_brake.cpp b/ArduSub/control_brake.cpp index b7f90df4bd..27c7adac5b 100644 --- a/ArduSub/control_brake.cpp +++ b/ArduSub/control_brake.cpp @@ -44,7 +44,7 @@ void Sub::brake_run() // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); - pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average); + pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-motors.get_throttle_hover()); return; } @@ -65,7 +65,7 @@ void Sub::brake_run() wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain()); // body-frame rate controller is run directly from 100hz loop diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index 0626444b8b..6a8c8541c4 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -80,9 +80,9 @@ void Sub::circle_run() // call attitude controller if (circle_pilot_yaw_override) { - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ - attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true); + attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain()); } // adjust climb rate using rangefinder diff --git a/ArduSub/control_drift.cpp b/ArduSub/control_drift.cpp index 5ee48a7f9a..837eb9fd1f 100644 --- a/ArduSub/control_drift.cpp +++ b/ArduSub/control_drift.cpp @@ -95,7 +95,7 @@ void Sub::drift_run() } // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // output pilot's throttle with angle boost attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt); diff --git a/ArduSub/control_flip.cpp b/ArduSub/control_flip.cpp index d16d505093..044ae6cc4b 100644 --- a/ArduSub/control_flip.cpp +++ b/ArduSub/control_flip.cpp @@ -177,7 +177,7 @@ void Sub::flip_run() case Flip_Recover: // use originally captured earth-frame angle targets to recover - attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false); + attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false, get_smoothing_gain()); // increase throttle to gain any lost altitude throttle_out += FLIP_THR_INC; diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index b413c8714c..ec863b69a3 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -177,7 +177,8 @@ bool Sub::guided_set_destination(const Vector3f& destination) #if AC_FENCE == ENABLED // reject destination if outside the fence - if (!fence.check_destination_within_fence(pv_alt_above_home(destination.z)*0.01f, pv_distance_to_home_cm(destination)*0.01f)) { + Location_Class dest_loc(destination); + if (!fence.check_destination_within_fence(dest_loc)) { Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; @@ -205,13 +206,10 @@ bool Sub::guided_set_destination(const Location_Class& dest_loc) #if AC_FENCE == ENABLED // reject destination outside the fence. // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails - int32_t alt_target_cm; - if (dest_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target_cm)) { - if (!fence.check_destination_within_fence(alt_target_cm*0.01f, get_distance_cm(ahrs.get_home(), dest_loc)*0.01f)) { - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); - // failure is propagated to GCS with NAK - return false; - } + if (!fence.check_destination_within_fence(dest_loc)) { + Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); + // failure is propagated to GCS with NAK + return false; } #endif @@ -337,7 +335,7 @@ void Sub::guided_takeoff_run() pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); } // guided_pos_control_run - runs the guided position controller @@ -375,10 +373,10 @@ void Sub::guided_pos_control_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true); + attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain()); } } @@ -422,10 +420,10 @@ void Sub::guided_vel_control_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true); + attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain()); } } @@ -491,10 +489,10 @@ void Sub::guided_posvel_control_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true); + attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain()); } } @@ -527,7 +525,7 @@ void Sub::guided_angle_control_run() float yaw_in = wrap_180_cd_float(guided_angle_state.yaw_cd); // constrain climb rate - float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabs(wp_nav.get_speed_down()), wp_nav.get_speed_up()); + float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav.get_speed_down()), wp_nav.get_speed_up()); // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds uint32_t tnow = millis(); @@ -541,7 +539,7 @@ void Sub::guided_angle_control_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); + attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain()); // call position controller pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false); diff --git a/ArduSub/control_land.cpp b/ArduSub/control_land.cpp index 904570a2ea..234e3be541 100644 --- a/ArduSub/control_land.cpp +++ b/ArduSub/control_land.cpp @@ -127,7 +127,7 @@ void Sub::land_gps_run() wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); // pause 4 seconds before beginning land descent float cmb_rate; @@ -192,7 +192,7 @@ void Sub::land_nogps_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // pause 4 seconds before beginning land descent float cmb_rate; diff --git a/ArduSub/control_rtl.cpp b/ArduSub/control_rtl.cpp index d91b8c2569..dd85982e4f 100644 --- a/ArduSub/control_rtl.cpp +++ b/ArduSub/control_rtl.cpp @@ -166,10 +166,10 @@ void Sub::rtl_climb_return_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true); + attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain()); } // check if we've completed this stage of RTL @@ -228,10 +228,10 @@ void Sub::rtl_loiterathome_run() // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true); + attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain()); } // check if we've completed this stage of RTL @@ -319,7 +319,7 @@ void Sub::rtl_descent_run() pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); // check if we've reached within 20cm of final altitude rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f; @@ -411,7 +411,7 @@ void Sub::rtl_land_run() desired_climb_rate = cmb_rate; // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); // check if we've completed this stage of RTL rtl_state_complete = ap.land_complete; diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index 8fb6b3a86e..63c10e8345 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -51,7 +51,7 @@ void Sub::stabilize_run() pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // body-frame rate controller is run directly from 100hz loop diff --git a/ArduSub/control_throw.cpp b/ArduSub/control_throw.cpp index 2abc3bed43..41ff351560 100644 --- a/ArduSub/control_throw.cpp +++ b/ArduSub/control_throw.cpp @@ -140,7 +140,7 @@ void Sub::throw_run() case Throw_Uprighting: // demand a level roll/pitch attitude with zero yaw rate - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain()); // output 50% throttle and turn off angle boost to maximise righting moment attitude_control.set_throttle_out(500, false, g.throttle_filt); @@ -150,7 +150,7 @@ void Sub::throw_run() case Throw_HgtStabilise: // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain()); // call height controller pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); @@ -164,7 +164,7 @@ void Sub::throw_run() wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain()); // call height controller pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); diff --git a/ArduSub/defines.h b/ArduSub/defines.h index bf3d60d417..f472756623 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -360,10 +360,11 @@ enum ThrowModeState { #define DATA_ROTOR_SPEED_BELOW_CRITICAL 59 // Heli only #define DATA_EKF_ALT_RESET 60 #define DATA_LAND_CANCELLED_BY_PILOT 61 -#define DATA_SURFACED 62 // Sub only -#define DATA_NOT_SURFACED 63 // Sub only -#define DATA_BOTTOMED 64 // Sub only -#define DATA_NOT_BOTTOMED 65 // Sub only +#define DATA_EKF_YAW_RESET 62 +#define DATA_SURFACED 63 // Sub only +#define DATA_NOT_SURFACED 64 // Sub only +#define DATA_BOTTOMED 65 // Sub only +#define DATA_NOT_BOTTOMED 66 // Sub only // Centi-degrees to radians #define DEGX100 5729.57795f diff --git a/ArduSub/land_detector.cpp b/ArduSub/land_detector.cpp index 4d2fba0507..3391d32dee 100644 --- a/ArduSub/land_detector.cpp +++ b/ArduSub/land_detector.cpp @@ -3,6 +3,11 @@ #include "Sub.h" #define LAND_END_DEPTH -20 //Landed at 20cm depth +// Code to detect a crash main ArduCopter code +#define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing +#define LAND_CHECK_LARGE_ANGLE_CD 1500.0f // maximum angle target to be considered landing +#define LAND_CHECK_ACCEL_MOVING 3.0f // maximum acceleration after subtracting gravity + // counter to verify landings static uint32_t land_detector_count = 0; diff --git a/ArduSub/make.inc b/ArduSub/make.inc index 24e13631b5..1e690b8faf 100644 --- a/ArduSub/make.inc +++ b/ArduSub/make.inc @@ -43,6 +43,7 @@ LIBRARIES += AC_WPNav LIBRARIES += AC_Circle LIBRARIES += AP_Declination LIBRARIES += AC_Fence +LIBRARIES += AC_Avoidance LIBRARIES += AP_Scheduler LIBRARIES += AP_RCMapper LIBRARIES += AP_Notify @@ -58,4 +59,5 @@ LIBRARIES += AP_RPM LIBRARIES += AC_PrecLand LIBRARIES += AP_IRLock LIBRARIES += AC_InputManager +LIBRARIES += AP_ADSB LIBRARIES += AP_JSButton diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 66a2928553..09a0339f41 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -191,6 +191,8 @@ void Sub::init_ardupilot() Location_Class::set_terrain(&terrain); wp_nav.set_terrain(&terrain); #endif + wp_nav.set_avoidance(&avoid); + pos_control.set_dt(MAIN_LOOP_SECONDS); // init the optical flow sensor diff --git a/ArduSub/wscript b/ArduSub/wscript index 3a53561638..be933c31cb 100644 --- a/ArduSub/wscript +++ b/ArduSub/wscript @@ -7,9 +7,11 @@ def build(bld): name=vehicle + '_libs', vehicle=vehicle, libraries=bld.ap_common_vehicle_libraries() + [ + 'AP_ADSB', 'AC_AttitudeControl', 'AC_InputManager', 'AC_Fence', + 'AC_Avoidance', 'AC_PID', 'AC_PrecLand', 'AC_Sprayer',