diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 3eb19b7104..0cc6fdc768 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -199,19 +199,17 @@ public: k_param_waypoint_mode = 210, // remove k_param_command_total, k_param_command_index, - k_param_command_nav_index, + k_param_command_nav_index, // remove k_param_waypoint_radius, - k_param_loiter_radius, + k_param_circle_radius, k_param_waypoint_speed_max, k_param_land_speed, // 217 // // 220: PI/D Controllers // - //k_param_stabilize_d_schedule = 219, - //k_param_stabilize_d = 220, k_param_acro_p = 221, - k_param_axis_lock_p, + k_param_axis_lock_p, // remove k_param_pid_rate_roll, k_param_pid_rate_pitch, k_param_pid_rate_yaw, @@ -276,12 +274,10 @@ public: // Waypoints // - AP_Int8 waypoint_mode; AP_Int8 command_total; AP_Int8 command_index; - AP_Int8 command_nav_index; AP_Int16 waypoint_radius; - AP_Int16 loiter_radius; + AP_Int16 circle_radius; AP_Int16 waypoint_speed_max; AP_Float crosstrack_gain; AP_Int16 crosstrack_min_distance; @@ -311,9 +307,7 @@ public: // Misc // AP_Int16 log_bitmask; - AP_Int16 log_last_filenumber; // *** Deprecated - remove - // with next eeprom number - // change + AP_Int8 toy_yaw_rate; // THOR The // Yaw Rate 1 // = fast, 2 = @@ -356,12 +350,8 @@ public: #endif AP_Int16 rc_speed; // speed of fast RC Channels in Hz - //AP_Float stabilize_d; - //AP_Float stabilize_d_schedule; - // Acro parameters AP_Float acro_p; - AP_Float axis_lock_p; AP_Int16 acro_balance_roll; AP_Int16 acro_balance_pitch; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 920b59ed7b..6feab241e4 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -14,9 +14,22 @@ #define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info : class::var_info} } const AP_Param::Info var_info[] PROGMEM = { + // @Param: SYSID_SW_MREV + // @DisplayName: Eeprom format version number + // @Description: This value is incremented when changes are made to the eeprom format + // @User: Advanced GSCALAR(format_version, "SYSID_SW_MREV", 0), + + // @Param: SYSID_SW_TYPE + // @DisplayName: Software Type + // @Description: This is used by the ground station to recognise the software type (eg ArduPlane vs ArduCopter) + // @User: Advanced GSCALAR(software_type, "SYSID_SW_TYPE", Parameters::k_software_type), + // @Param: SYSID_THISMAV + // @DisplayName: Mavlink version + // @Description: Allows reconising the mavlink version + // @User: Advanced GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID), GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), @@ -39,7 +52,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: ALT_RTL // @DisplayName: RTL Altitude // @Description: The minimum altitude the model will move to before Returning to Launch. Set to zero to return at current altitude. - // @Units: centimeters + // @Units: Centimeters // @Range: 0 4000 // @Increment: 1 // @User: Standard @@ -52,8 +65,18 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(sonar_enabled, "SONAR_ENABLE", DISABLED), + // @Param: SONAR_TYPE + // @DisplayName: Sonar type + // @Description: Used to adjust scaling to match the sonar used (only Maxbotix sonars are supported at this time) + // @Values: 0:XL-EZ0,1:LV-EZ0,2:XLL-EZ0,3:HRLV + // @User: Standard GSCALAR(sonar_type, "SONAR_TYPE", AP_RANGEFINDER_MAXSONARXL), + // @Param: BATT_MONITOR + // @DisplayName: Battery monitoring + // @Description: Controls enabling monitoring of the battery's voltage and current + // @Values: 0:Disabled,3:Voltage Only,4:Voltage and Current + // @User: Standard GSCALAR(battery_monitoring, "BATT_MONITOR", DISABLED), // @Param: BATT_FAILSAFE @@ -65,10 +88,20 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: VOLT_DIVIDER // @DisplayName: Voltage Divider - // @Description: TODO + // @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin voltage * INPUT_VOLTS/1024 * VOLT_DIVIDER) + // @User: Advanced GSCALAR(volt_div_ratio, "VOLT_DIVIDER", VOLT_DIV_RATIO), + // @Param: AMP_PER_VOLT + // @DisplayName: Current Amps per volt + // @Description: Used to convert the voltage on the current sensing pin (BATT_CURR_PIN) to the actual current being consumed in amps (curr pin voltage * INPUT_VOLTS/1024 * AMP_PER_VOLT ) + // @User: Advanced GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT", CURR_AMP_PER_VOLT), + + // @Param: INPUT_VOLTS + // @DisplayName: Max internal voltage of the battery voltage and current sensing pins + // @Description: Used to convert the voltage read in on the voltage and current pins for battery monitoring. Normally 5 meaning 5 volts. + // @User: Advanced GSCALAR(input_voltage, "INPUT_VOLTS", INPUT_VOLTAGE), // @Param: BATT_CAPACITY @@ -109,7 +142,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: RTL_ALT_FINAL // @DisplayName: RTL Final Altitude // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to -1 to disable, zero to land. - // @Units: centimeters + // @Units: Centimeters // @Range: -1 1000 // @Increment: 1 // @User: Standard @@ -158,10 +191,17 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(yaw_override_behaviour, "YAW_OVR_BEHAVE", YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT), - GSCALAR(waypoint_mode, "WP_MODE", 0), + // @Param: WP_TOTAL + // @DisplayName: Waypoint Total + // @Description: Total number of commands in the mission stored in the eeprom. Do not update this parameter directly! + // @User: Advanced GSCALAR(command_total, "WP_TOTAL", 0), + + // @Param: WP_INDEX + // @DisplayName: Waypoint Index + // @Description: The index number of the command that is currently being executed. Do not update this parameter directly! + // @User: Advanced GSCALAR(command_index, "WP_INDEX", 0), - GSCALAR(command_nav_index, "WP_MUST_INDEX",0), // @Param: WP_RADIUS // @DisplayName: Waypoint Radius @@ -172,14 +212,14 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(waypoint_radius, "WP_RADIUS", WP_RADIUS_DEFAULT), - // @Param: WP_LOITER_RAD - // @DisplayName: Waypoint Loiter Radius - // @Description: Defines the distance from the waypoint center, the vehicle will maintain during a loiter + // @Param: CIRCLE_RADIUS + // @DisplayName: Circle radius + // @Description: Defines the radius of the circle the vehicle will fly when in Circle flight mode // @Units: Meters // @Range: 1 127 // @Increment: 1 // @User: Standard - GSCALAR(loiter_radius, "WP_LOITER_RAD", LOITER_RADIUS), + GSCALAR(circle_radius, "CIRCLE_RADIUS", CIRCLE_RADIUS), // @Param: WP_SPEED_MAX // @DisplayName: Waypoint Max Speed Target @@ -217,7 +257,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: LAND_SPEED // @DisplayName: Land speed // @Description: The descent speed for the final stage of landing in cm/s - // @Units: cm/s + // @Units: Centimeters/Second // @Range: 10 200 // @Increment: 10 // @User: Standard @@ -225,7 +265,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: THR_MIN // @DisplayName: Minimum Throttle - // @Description: The minimum throttle which the autopilot will apply. + // @Description: The minimum throttle that will be sent to the motors to keep them spinning // @Units: ms // @Range: 0 1000 // @Increment: 1 @@ -234,7 +274,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: THR_MAX // @DisplayName: Maximum Throttle - // @Description: The maximum throttle which the autopilot will apply. + // @Description: The maximum throttle that will be sent to the motors // @Units: ms // @Range: 0 1000 // @Increment: 1 @@ -256,35 +296,105 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(throttle_fs_value, "THR_FS_VALUE", THROTTLE_FS_VALUE), + // @Param: TRIM_THROTTLE + // @DisplayName: Throttle Trim + // @Description: The PWM level on channel 3 below which throttle sailsafe triggers + // @User: Standard GSCALAR(throttle_cruise, "TRIM_THROTTLE", THROTTLE_CRUISE), + // @Param: FLTMODE1 + // @DisplayName: Flight Mode 1 + // @Description: Flight mode when Channel 5 pwm is <= 1230 + // @User: Standard GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1), + + // @Param: FLTMODE2 + // @DisplayName: Flight Mode 2 + // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 + // @User: Standard GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2), + + // @Param: FLTMODE3 + // @DisplayName: Flight Mode 3 + // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490 + // @User: Standard GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3), + + // @Param: FLTMODE4 + // @DisplayName: Flight Mode 4 + // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620 + // @User: Standard GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4), + + // @Param: FLTMODE5 + // @DisplayName: Flight Mode 5 + // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749 + // @User: Standard GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5), + + // @Param: FLTMODE6 + // @DisplayName: Flight Mode 6 + // @Description: Flight mode when Channel 5 pwm is >=1750 + // @User: Standard GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), + + // @Param: SIMPLE + // @DisplayName: Simple mode bitmask + // @Description: Bitmask which holds which flight modes use simple heading mode (eg bit 0 = 1 means Flight Mode 0 uses simple mode) + // @User: Advanced GSCALAR(simple_modes, "SIMPLE", 0), // @Param: LOG_BITMASK // @DisplayName: Log bitmask - // @Description: bitmap of log fields to enable + // @Description: 2 byte bitmap of log types to enable // @User: Advanced GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), - GSCALAR(log_last_filenumber, "LOG_LASTFILE", 0), - // THOR - // Added to allow change of Rate in the Mission planner + + // @Param: TOY_RATE + // @DisplayName: Toy Yaw Rate + // @Description: Controls yaw rate in Toy mode. Higher values will cause a slower yaw rate. Do not set to zero! + // @User: Advanced + // @Range: 1 10 GSCALAR(toy_yaw_rate, "TOY_RATE", 1), + // @Param: ESC + // @DisplayName: ESC Calibration + // @Description: Controls whether ArduCopter will enter ESC calibration on the next restart. Do not adjust this parameter manually. + // @User: Advanced + // @Values: 0:Normal Start-up,1:Start-up in ESC Calibration mode GSCALAR(esc_calibrate, "ESC", 0), + + // @Param: TUNE + // @DisplayName: Channel 6 Tuning + // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob + // @User: Standard + // @Values: 0:CH6_NONE,1:CH6_STABILIZE_KP,2:CH6_STABILIZE_KI,3:CH6_YAW_KP,4:CH6_RATE_KP,5:CH6_RATE_KI,6:CH6_YAW_RATE_KP,7:CH6_THROTTLE_KP,8:CH6_TOP_BOTTOM_RATIO,9:CH6_RELAY,10:CH6_TRAVERSE_SPEED,11:CH6_NAV_KP,12:CH6_LOITER_KP,13:CH6_HELI_EXTERNAL_GYRO,14:CH6_THR_HOLD_KP,17:CH6_OPTFLOW_KP,18:CH6_OPTFLOW_KI,19:CH6_OPTFLOW_KD,20:CH6_NAV_KI,21:CH6_RATE_KD,22:CH6_LOITER_RATE_KP,23:CH6_LOITER_RATE_KD,24:CH6_YAW_KI,25:CH6_ACRO_KP,26:CH6_YAW_RATE_KD,27:CH6_LOITER_KI,28:CH6_LOITER_RATE_KI,29:CH6_STABILIZE_KD,30:CH6_AHRS_YAW_KP,31:CH6_AHRS_KP,32:CH6_INAV_TC,33:CH6_THROTTLE_KI,34:CH6_THR_ACCEL_KP,35:CH6_THR_ACCEL_KI,36:CH6_THR_ACCEL_KD GSCALAR(radio_tuning, "TUNE", 0), + + // @Param: TUNE_LOW + // @DisplayName: Tuning minimum + // @Description: The minimum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob + // @User: Standard + // @Range: 0 32767 GSCALAR(radio_tuning_low, "TUNE_LOW", 0), + + // @Param: TUNE_HIGH + // @DisplayName: Tuning maximum + // @Description: The maximum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob + // @User: Standard + // @Range: 0 32767 GSCALAR(radio_tuning_high, "TUNE_HIGH", 1000), + + // @Param: FRAME + // @DisplayName: Frame Orientation + // @Description: Congrols motor mixing The maximum value that will be applied to the parameter currently being tuned with the transmitter's channel 6 knob + // @User: Standard + // @Range: 0 32767 GSCALAR(frame_orientation, "FRAME", FRAME_ORIENTATION), // @Param: CH7_OPT // @DisplayName: Channel 7 option - // @Description: Select which function if performed when CH7 is high + // @Description: Select which function if performed when CH7 is above 1800 pwm // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger // @User: Standard GSCALAR(ch7_option, "CH7_OPT", CH7_OPTION), @@ -359,22 +469,20 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(rc_speed, "RC_SPEED", RC_FAST_SPEED), - // variable - //--------- - //GSCALAR(stabilize_d, "STAB_D", STABILIZE_D), - - // @Param: STAB_D_S - // @DisplayName: Stabilize D Schedule - // @Description: This value is a percentage of scheduling applied to the Stabilize D term. - // @Range: 0 1 - // @Increment: .01 - // @User: Advanced - //GSCALAR(stabilize_d_schedule, "STAB_D_S", STABILIZE_D_SCHEDULE), - - // Acro parameters + // @Param: ACRO_P + // @DisplayName: Acro P gain + // @Description: Used to convert pilot roll, pitch and yaw input into a dssired rate of rotation in ACRO mode. Higher values mean faster rate of rotation. + // @Range: 1 10 + // @User: Standard GSCALAR(acro_p, "ACRO_P", ACRO_P), - GSCALAR(axis_lock_p, "AXIS_P", AXIS_LOCK_P), + + // @Param: AXIS_ENABLE + // @DisplayName: Acro Axis + // @Description: Used to control whether acro mode actively maintains the current angle when control sticks are released (Enabled = maintains current angle) + // @Values: 0:Disabled, 1:Enabled + // @User: Standard GSCALAR(axis_enabled, "AXIS_ENABLE", AXIS_LOCK_ENABLED), + // @Param: ACRO_BAL_ROLL // @DisplayName: Acro Balance Roll // @Description: rate at which roll angle returns to level in acro mode @@ -391,6 +499,11 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(acro_balance_pitch, "ACRO_BAL_PITCH", ACRO_BALANCE_PITCH), + // @Param: LED_MODE + // @DisplayName: Copter LED Mode + // @Description: bitmap to control the copter led mode + // @Values: 0:Disabled,1:Enable,2:GPS On,4:Aux,8:Buzzer,16:Oscillate,32:Nav Blink,64:GPS Nav Blink + // @User: Standard GSCALAR(copter_leds_mode, "LED_MODE", 9), // PID controller diff --git a/ArduCopter/config.h b/ArduCopter/config.h index ba650d394a..3a0fc33990 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -667,16 +667,10 @@ # define ACRO_P 4.5 #endif - #ifndef AXIS_LOCK_ENABLED # define AXIS_LOCK_ENABLED ENABLED #endif -#ifndef AXIS_LOCK_P - # define AXIS_LOCK_P .02 -#endif - - // Good for smaller payload motors. #ifndef STABILIZE_ROLL_P # define STABILIZE_ROLL_P 4.5 @@ -763,15 +757,6 @@ #endif -#ifndef STABILIZE_D - # define STABILIZE_D 0.00 -#endif - -#ifndef STABILIZE_D_SCHEDULE - # define STABILIZE_D_SCHEDULE 0.5 -#endif - - ////////////////////////////////////////////////////////////////////////////// // Rate controlled stabilized variables // @@ -1052,8 +1037,8 @@ # define WP_RADIUS_DEFAULT 2 #endif -#ifndef LOITER_RADIUS - # define LOITER_RADIUS 10 // meters for circle mode +#ifndef CIRCLE_RADIUS + # define CIRCLE_RADIUS 10 // meters for circle mode #endif #ifndef USE_CURRENT_ALT diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index a28c0d23f5..5497368812 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -298,8 +298,8 @@ static void update_nav_wp() if (circle_angle > 6.28318531) circle_angle -= 6.28318531; - next_WP.lng = circle_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp); - next_WP.lat = circle_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle)); + next_WP.lng = circle_WP.lng + (g.circle_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp); + next_WP.lat = circle_WP.lat + (g.circle_radius * 100 * sin(1.57 - circle_angle)); // use error as the desired rate towards the target // nav_lon, nav_lat is calculated