From d174a2f190a6cdd5c3d15e06adb03531631d7e57 Mon Sep 17 00:00:00 2001 From: z Date: Wed, 29 Apr 2020 17:15:29 +0800 Subject: [PATCH] edit param value --- ArduCopter/AP_Arming.cpp | 12 ++--- ArduCopter/Parameters.cpp | 52 ++++----------------- ArduCopter/Parameters.h | 6 +-- ArduCopter/config.h | 16 +++---- libraries/AC_Avoidance/AC_Avoid.h | 2 +- libraries/AC_WPNav/AC_Loiter.cpp | 2 +- libraries/AC_WPNav/AC_WPNav.h | 4 +- libraries/AP_BoardConfig/AP_BoardConfig.cpp | 4 +- libraries/AP_Camera/AP_Camera.cpp | 2 +- libraries/AP_Camera/AP_Camera.h | 4 +- libraries/AP_Compass/AP_Compass.cpp | 4 +- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 4 +- libraries/AP_Relay/AP_Relay.cpp | 2 +- 13 files changed, 39 insertions(+), 75 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 7397327d13..490ae975af 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -562,6 +562,11 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) { const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf(); + //arm if Trial time is not arrived add@bnsir + if( !mandatory_deadline_checks(false)){ + return false; + } + // always check if inertial nav has started and is ready if (!ahrs.healthy()) { check_failed(true, "AHRS not healthy"); @@ -580,13 +585,6 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) } #endif - //arm if Trial time is not arrived add@bnsir - -if( !mandatory_deadline_checks(false)){ - return false; -} - - Mode::Number control_mode = copter.control_mode; // always check if the current mode allows arming diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index f636c9d5ba..1d7cce080c 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -43,24 +43,18 @@ const AP_Param::Info Copter::var_info[] = { // @ReadOnly: True GSCALAR(format_version, "SYSID_SW_MREV", 0), - // @Param: BOARD_ID - // @DisplayName: Board ID - // @Description: For mark board id - // @User: @Brown - GSCALAR(sysid_board_id, "SYSID_BOARD_ID", 100), - // @Param: SYSID_BD_NAME1 // @DisplayName: Board Name 1st // @Description: For mark board name first // @Range: 1-4294967269 // @User: @Binsir - GSCALAR(sysid_board_name_1st, "SYSID_BDNAME_P1", 12848 ), + GSCALAR(sysid_board_name_1st, "SYSID_BDNAME_P1", 2004 ), // @Param: SYSID_BD_NAME2 // @DisplayName: Board Name 2nd // @Description: For mark board name last // @User: @Binsir - GSCALAR(sysid_board_name_2nd, "SYSID_BDNAME_P2", 5329970), + GSCALAR(sysid_board_name_2nd, "SYSID_BDNAME_P2", 1001), // @Param: SYSID_THISMAV // @DisplayName: MAVLink system ID of this vehicle @@ -220,7 +214,7 @@ const AP_Param::Info Copter::var_info[] = { // @Description: Determines how the autopilot controls the yaw during missions and RTL // @Values: 0:Never change yaw, 1:Face next waypoint, 2:Face next waypoint except RTL, 3:Face along GPS course // @User: Standard - GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_DEFAULT), + GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP), // @Param: LAND_SPEED // @DisplayName: Land speed @@ -229,7 +223,7 @@ const AP_Param::Info Copter::var_info[] = { // @Range: 30 200 // @Increment: 10 // @User: Standard - GSCALAR(land_speed, "LAND_SPEED", LAND_SPEED), + GSCALAR(land_speed, "LAND_SPEED", LAND_SPEED), // @Param: LAND_SPEED_HIGH // @DisplayName: Land speed high @@ -263,7 +257,7 @@ const AP_Param::Info Copter::var_info[] = { // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Deprecated in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land // @User: Standard - GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL), + GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_DISABLED), // @Param: FS_THR_VALUE // @DisplayName: Throttle Failsafe Value @@ -910,7 +904,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Range: 100 10000 // @Increment: 10 // @User: Advanced - AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 1000), + AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 2000), #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED // @Group: FHLD @@ -1629,39 +1623,11 @@ const char* Copter::get_sysid_board_id(void) int32_t nameValue1 =(int32_t) g.sysid_board_name_1st; int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd; - char name[7] ; - memset(name,0,7); - //nameValue2 = nameValue2 & 0xffffff; - name[5] = '.'; - name[4] = nameValue2&0xFF; - name[3] = nameValue2>>8&0xFF; - name[2] = nameValue2>>16& 0xFF; - name[1] = nameValue1&0xFF; - name[0] = nameValue1>>8 & 0xFF; - snprintf(buf, sizeof(buf), "Version: zr-v4.0.1 ,Board ID: ZRZK.%s%d",name,(int)g.sysid_board_id); + + // snprintf(buf, sizeof(buf), "Version: zr-v4.0.1 ,Board ID: ZRZK.%s%d",name,(int)g.sysid_board_id); + snprintf(buf, sizeof(buf), "Version: zr-v4.0.1 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2); return buf; } -// // modify by @Brown -// const char* Copter::get_sysid_board_id(void) -// { - -// static char buf[50]; -// // snprintf(buf, sizeof(buf), "Version: v3.5.6 ,Board ID: ZRZK.19QT3.%d",(int)g.sysid_board_id); - -// int32_t nameValue1 =(int32_t) g.sysid_board_name_1st; -// int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd; -// char name[7] = {' ',' ',' ',' ',' '}; -// // memset(name,0,6); -// nameValue2 = nameValue2 & 0xffffff; -// // name[5] = "a"; -// name[4] = nameValue2&0xFF; -// name[3] = nameValue2>>8&0xFF; -// name[2] = nameValue2>>16& 0xFF; -// name[1] = nameValue1&0xFF; -// name[0] = nameValue1>>8 & 0xFF; -// snprintf(buf, sizeof(buf), "Version: zr-v4.0.1,Board ID: ZRZK.%s.%d",name,(int)g.sysid_board_id); -// return buf; -// } void Copter::get_deadline_params(int32_t &deadline) { diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e4723902dd..b83b00caed 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -387,9 +387,9 @@ public: AP_Int16 format_version; - AP_Int16 sysid_board_id; // modify by @Brown - AP_Int32 sysid_board_name_1st;// modify by @Binsir - AP_Int32 sysid_board_name_2nd;// modify by @Binsir + // AP_Int32 sysid_board_id; // modify by @Brown + AP_Int16 sysid_board_name_1st;// modify by @Binsir + AP_Int16 sysid_board_name_2nd;// modify by @Binsir // Telemetry control // diff --git a/ArduCopter/config.h b/ArduCopter/config.h index ea280c80b0..12f66ba017 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -410,22 +410,22 @@ // #ifndef FLIGHT_MODE_1 - # define FLIGHT_MODE_1 Mode::Number::STABILIZE + # define FLIGHT_MODE_1 Mode::Number::ALT_HOLD #endif #ifndef FLIGHT_MODE_2 - # define FLIGHT_MODE_2 Mode::Number::STABILIZE + # define FLIGHT_MODE_2 Mode::Number::ALT_HOLD #endif #ifndef FLIGHT_MODE_3 - # define FLIGHT_MODE_3 Mode::Number::STABILIZE + # define FLIGHT_MODE_3 Mode::Number::LOITER #endif #ifndef FLIGHT_MODE_4 - # define FLIGHT_MODE_4 Mode::Number::STABILIZE + # define FLIGHT_MODE_4 Mode::Number::LOITER #endif #ifndef FLIGHT_MODE_5 - # define FLIGHT_MODE_5 Mode::Number::STABILIZE + # define FLIGHT_MODE_5 Mode::Number::AUTO #endif #ifndef FLIGHT_MODE_6 - # define FLIGHT_MODE_6 Mode::Number::STABILIZE + # define FLIGHT_MODE_6 Mode::Number::AUTO #endif @@ -504,7 +504,7 @@ #endif #ifndef ACRO_YAW_P - # define ACRO_YAW_P 4.5f + # define ACRO_YAW_P 3.0f #endif #ifndef ACRO_LEVEL_MAX_ANGLE @@ -641,7 +641,7 @@ #endif #ifndef AUTO_DISARMING_DELAY -# define AUTO_DISARMING_DELAY 10 +# define AUTO_DISARMING_DELAY 5 #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index dbf554b22d..927b0c9929 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -15,7 +15,7 @@ #define AC_AVOID_DEFAULT (AC_AVOID_STOP_AT_FENCE | AC_AVOID_USE_PROXIMITY_SENSOR) // definitions for non-GPS avoidance -#define AC_AVOID_NONGPS_DIST_MAX_DEFAULT 5.0f // objects over 5m away are ignored (default value for DIST_MAX parameter) +#define AC_AVOID_NONGPS_DIST_MAX_DEFAULT 7.0f // objects over 5m away are ignored (default value for DIST_MAX parameter) #define AC_AVOID_ANGLE_MAX_PERCENT 0.75f // object avoidance max lean angle as a percentage (expressed in 0 ~ 1 range) of total vehicle max lean angle /* diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 38647ac45f..9143edcb24 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -3,7 +3,7 @@ extern const AP_HAL::HAL& hal; -#define LOITER_SPEED_DEFAULT 1250.0f // default loiter speed in cm/s +#define LOITER_SPEED_DEFAULT 1000.0f // default loiter speed in cm/s #define LOITER_SPEED_MIN 20.0f // minimum loiter speed in cm/s #define LOITER_ACCEL_MAX_DEFAULT 500.0f // default acceleration in loiter mode #define LOITER_BRAKE_ACCEL_DEFAULT 250.0f // minimum acceleration in loiter mode diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index a440188faf..88af53c079 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -14,13 +14,13 @@ #define WPNAV_ACCELERATION 100.0f // defines the default velocity vs distant curve. maximum acceleration in cm/s/s that position controller asks for from acceleration controller #define WPNAV_ACCELERATION_MIN 50.0f // minimum acceleration in cm/s/s - used for sanity checking _wp_accel parameter -#define WPNAV_WP_SPEED 500.0f // default horizontal speed between waypoints in cm/s +#define WPNAV_WP_SPEED 800.0f // default horizontal speed between waypoints in cm/s #define WPNAV_WP_SPEED_MIN 20.0f // minimum horizontal speed between waypoints in cm/s #define WPNAV_WP_TRACK_SPEED_MIN 50.0f // minimum speed along track of the target point the vehicle is chasing in cm/s (used as target slows down before reaching destination) #define WPNAV_WP_RADIUS 200.0f // default waypoint radius in cm #define WPNAV_WP_RADIUS_MIN 5.0f // minimum waypoint radius in cm -#define WPNAV_WP_SPEED_UP 250.0f // default maximum climb velocity +#define WPNAV_WP_SPEED_UP 350.0f // default maximum climb velocity #define WPNAV_WP_SPEED_DOWN 150.0f // default maximum descent velocity #define WPNAV_WP_ACCEL_Z_DEFAULT 100.0f // default vertical acceleration between waypoints in cm/s/s diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index a4615679cf..ba913d6b5c 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -38,7 +38,7 @@ #endif #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -# define BOARD_SAFETY_ENABLE_DEFAULT 1 +# define BOARD_SAFETY_ENABLE_DEFAULT 0 #ifndef BOARD_PWM_COUNT_DEFAULT # define BOARD_PWM_COUNT_DEFAULT 6 #endif @@ -62,7 +62,7 @@ #endif #ifndef BOARD_PWM_COUNT_DEFAULT -#define BOARD_PWM_COUNT_DEFAULT 8 +#define BOARD_PWM_COUNT_DEFAULT 4 #endif #ifndef BOARD_CONFIG_BOARD_VOLTAGE_MIN diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 106a431434..4f328c8324 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -97,7 +97,7 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = { // @Description: When enabled, trigging by distance is done in AUTO mode only. // @Values: 0:Always,1:Only when in AUTO // @User: Standard - AP_GROUPINFO("AUTO_ONLY", 10, AP_Camera, _auto_mode_only, 0), + AP_GROUPINFO("AUTO_ONLY", 10, AP_Camera, _auto_mode_only, 1), // @Param: TYPE // @DisplayName: Type of camera (0: None, 1: BMMCC) diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index ea08d4ce59..80d1d219eb 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -8,9 +8,9 @@ #define AP_CAMERA_TRIGGER_TYPE_SERVO 0 #define AP_CAMERA_TRIGGER_TYPE_RELAY 1 -#define AP_CAMERA_TRIGGER_DEFAULT_TRIGGER_TYPE AP_CAMERA_TRIGGER_TYPE_SERVO // default is to use servo to trigger camera +#define AP_CAMERA_TRIGGER_DEFAULT_TRIGGER_TYPE AP_CAMERA_TRIGGER_TYPE_RELAY // default is to use servo to trigger camera -#define AP_CAMERA_TRIGGER_DEFAULT_DURATION 10 // default duration servo or relay is held open in 10ths of a second (i.e. 10 = 1 second) +#define AP_CAMERA_TRIGGER_DEFAULT_DURATION 3 // default duration servo or relay is held open in 10ths of a second (i.e. 10 = 1 second) #define AP_CAMERA_SERVO_ON_PWM 1300 // default PWM value to move servo to when shutter is activated #define AP_CAMERA_SERVO_OFF_PWM 1100 // default PWM value to move servo to when shutter is deactivated diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 5cf9e6dc2a..f48f984c74 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -296,7 +296,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Description: Enable or disable the second compass for determining heading. // @Values: 0:Disabled,1:Enabled // @User: Advanced - AP_GROUPINFO("USE2", 18, Compass, _state[1].use_for_yaw, 1), + AP_GROUPINFO("USE2", 18, Compass, _state[1].use_for_yaw, 0), // @Param: ORIENT2 // @DisplayName: Compass2 orientation @@ -319,7 +319,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Description: Enable or disable the third compass for determining heading. // @Values: 0:Disabled,1:Enabled // @User: Advanced - AP_GROUPINFO("USE3", 21, Compass, _state[2].use_for_yaw, 1), + AP_GROUPINFO("USE3", 21, Compass, _state[2].use_for_yaw, 0), // @Param: ORIENT3 // @DisplayName: Compass3 orientation diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 12a0d75f5a..1594f33835 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -107,7 +107,7 @@ #define VEL_I_GATE_DEFAULT 500 #define POS_I_GATE_DEFAULT 500 #define HGT_I_GATE_DEFAULT 500 -#define MAG_I_GATE_DEFAULT 300 +#define MAG_I_GATE_DEFAULT 800 #define MAG_CAL_DEFAULT 3 #define GLITCH_RADIUS_DEFAULT 25 #define FLOW_MEAS_DELAY 10 @@ -452,7 +452,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @Range: 100 1000 // @Increment: 25 // @User: Advanced - AP_GROUPINFO("YAW_I_GATE", 38, NavEKF2, _yawInnovGate, 300), + AP_GROUPINFO("YAW_I_GATE", 38, NavEKF2, _yawInnovGate, 800), // @Param: TAU_OUTPUT // @DisplayName: Output complementary filter time constant (centi-sec) diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index 47c7349a34..8c32e9d9e5 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -21,7 +21,7 @@ #endif #ifndef RELAY1_PIN_DEFAULT - #define RELAY1_PIN_DEFAULT -1 + #define RELAY1_PIN_DEFAULT 56 #endif #ifndef RELAY2_PIN_DEFAULT