diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 07020c5ed1..29b38c268f 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -17,14 +17,8 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 14; - - // The parameter software_type is set up solely for ground station use - // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) - // GCS will interpret values 0-9 as ArduPilotMega. Developers may use - // values within that range to identify different branches. - // - static const uint16_t k_software_type = 20; // 0 for APM trunk + static const uint16_t k_format_version = 15; + static const uint16_t k_software_type = 20; enum { // Layout version number, always key zero. @@ -34,13 +28,12 @@ public: // Misc // - k_param_log_bitmask, + k_param_log_bitmask = 10, k_param_num_resets, - k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change k_param_reset_switch_chan, - k_param_manual_level, - k_param_ins, // libraries/AP_InertialSensor variables - k_param_rssi_pin, + + // IO pins + k_param_rssi_pin = 20, k_param_battery_volt_pin, k_param_battery_curr_pin, @@ -58,22 +51,15 @@ public: // // 130: Sensor parameters // - k_param_imu = 130, - k_param_compass_enabled, - k_param_compass, - k_param_battery_monitoring, + k_param_compass_enabled = 130, + + // 140: battery controls + k_param_battery_monitoring = 140, k_param_volt_div_ratio, k_param_curr_amp_per_volt, k_param_input_voltage, k_param_pack_capacity, -#if CONFIG_SONAR == ENABLED - k_param_sonar_enabled, - k_param_sonar_type, -#endif - - k_param_ahrs, // AHRS group - // // 150: Navigation parameters // @@ -94,22 +80,27 @@ public: k_param_rc_7, k_param_rc_8, - k_param_throttle_min, + // throttle control + k_param_throttle_min = 170, k_param_throttle_max, - k_param_throttle_fs_enabled, // 170 - k_param_throttle_fs_value, k_param_throttle_cruise, - - k_param_long_fs_action, - k_param_gcs_heartbeat_fs_enabled, k_param_throttle_slewrate, + k_param_throttle_reduction, - // 180: APMrover parameters - JLN update - k_param_sonar_trigger, - k_param_booster, + // failsafe control + k_param_fs_action = 180, + k_param_fs_timeout, + k_param_fs_throttle_enabled, + k_param_fs_throttle_value, + k_param_fs_gcs_enabled, + + // obstacle control + k_param_sonar_trigger = 190, + k_param_sonar_enabled, + k_param_sonar_type, // - // 210: flight modes + // 210: driving modes // k_param_mode_channel = 210, k_param_mode1, @@ -122,30 +113,39 @@ public: // // 220: Waypoint data // - k_param_waypoint_mode = 220, - k_param_command_total, + k_param_command_total = 220, k_param_command_index, k_param_waypoint_radius, - // other objects - k_param_sitl = 230, - // // 240: PID Controllers - k_param_pidNavSteer = 240, - - // steering-to-servo PID: + k_param_pidNavSteer = 230, k_param_pidServoSteer, - - // steering-to-servo PID: k_param_pidSpeedThrottle, + // other objects + k_param_sitl = 240, + k_param_ahrs, + k_param_ins, + k_param_compass, + // 254,255: reserved }; AP_Int16 format_version; AP_Int8 software_type; + // Misc + // + AP_Int16 log_bitmask; + AP_Int16 num_resets; + AP_Int8 reset_switch_chan; + + // IO pins + AP_Int8 rssi_pin; + AP_Int8 battery_volt_pin; + AP_Int8 battery_curr_pin; + // Telemetry control // AP_Int16 sysid_this_mav; @@ -154,77 +154,22 @@ public: AP_Int8 serial3_baud; AP_Int8 telem_delay; - // Crosstrack navigation - // - AP_Float crosstrack_gain; - AP_Int16 crosstrack_entry_angle; - - // Waypoints - // - AP_Int8 waypoint_mode; - AP_Int8 command_total; - AP_Int8 command_index; - AP_Float waypoint_radius; - - // Throttle - // - AP_Int8 throttle_min; - AP_Int8 throttle_max; - AP_Int8 throttle_slewrate; - AP_Int8 throttle_fs_enabled; - AP_Int16 throttle_fs_value; - AP_Int8 throttle_cruise; - - // Failsafe - AP_Int8 long_fs_action; - AP_Int8 gcs_heartbeat_fs_enabled; - - // Flight modes - // - AP_Int8 mode_channel; - AP_Int8 mode1; - AP_Int8 mode2; - AP_Int8 mode3; - AP_Int8 mode4; - AP_Int8 mode5; - AP_Int8 mode6; - - // Misc - // - AP_Int16 num_resets; - AP_Int16 log_bitmask; - AP_Int16 log_last_filenumber; // *** Deprecated - remove with next eeprom number change - AP_Int8 reset_switch_chan; - AP_Int8 manual_level; - AP_Float speed_cruise; - AP_Int8 ch7_option; - + // sensor parameters AP_Int8 compass_enabled; + + // battery controls AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current AP_Float volt_div_ratio; AP_Float curr_amp_per_volt; AP_Float input_voltage; - AP_Int16 pack_capacity; // Battery pack capacity less reserve - AP_Int8 rssi_pin; - AP_Int8 battery_volt_pin; - AP_Int8 battery_curr_pin; + AP_Int16 pack_capacity; // Battery pack capacity less reserve -#if HIL_MODE != HIL_MODE_ATTITUDE -#if CONFIG_SONAR == ENABLED - AP_Int8 sonar_enabled; - AP_Int8 sonar_type; // 0 = XL, 1 = LV, - // 2 = XLL (XL with 10m range) - // 3 = HRLV -#endif -#endif - -// ************ ThermoPilot parameters ************************ -// - JLN update - - AP_Float sonar_trigger; - AP_Int8 booster; - -// ************************************************************ + // navigation parameters + // + AP_Float crosstrack_gain; + AP_Int16 crosstrack_entry_angle; + AP_Float speed_cruise; + AP_Int8 ch7_option; // RC channels RC_Channel channel_steer; @@ -236,6 +181,43 @@ public: RC_Channel_aux rc_7; RC_Channel_aux rc_8; + // Throttle + // + AP_Int8 throttle_min; + AP_Int8 throttle_max; + AP_Int8 throttle_cruise; + AP_Int8 throttle_slewrate; + + // failsafe control + AP_Int8 fs_action; + AP_Float fs_timeout; + AP_Int8 fs_throttle_enabled; + AP_Int16 fs_throttle_value; + AP_Int8 fs_gcs_enabled; + + // obstacle control + AP_Float sonar_trigger; + AP_Int8 sonar_enabled; + AP_Int8 sonar_type; // 0 = XL, 1 = LV, 2 = XLL (XL with 10m + // range), + // 3 = HRLV + + // driving modes + // + AP_Int8 mode_channel; + AP_Int8 mode1; + AP_Int8 mode2; + AP_Int8 mode3; + AP_Int8 mode4; + AP_Int8 mode5; + AP_Int8 mode6; + + // Waypoints + // + AP_Int8 command_total; + AP_Int8 command_index; + AP_Float waypoint_radius; + // PID controllers // PID pidNavSteer; @@ -255,9 +237,9 @@ public: // PID controller initial P initial I initial D initial imax //----------------------------------------------------------------------------------- - pidNavSteer (0.7, 0.1, 0.2, 200), - pidServoSteer (0.5, 0.1, 0.2, 200), - pidSpeedThrottle (0.7, 0.2, 0.2, 200) + pidNavSteer (0.7, 0.1, 0.2, 2000), + pidServoSteer (0.5, 0.1, 0.2, 2000), + pidSpeedThrottle (0.7, 0.2, 0.2, 4000) {} }; diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 96fd3e6fe9..07ded009ed 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -16,7 +16,45 @@ const AP_Param::Info var_info[] PROGMEM = { GSCALAR(format_version, "FORMAT_VERSION", 1), GSCALAR(software_type, "SYSID_SW_TYPE", Parameters::k_software_type), + + // misc + GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), + GSCALAR(num_resets, "SYS_NUM_RESETS", 0), + GSCALAR(reset_switch_chan, "RST_SWITCH_CH", 0), + + // @Param: RSSI_PIN + // @DisplayName: Receiver RSSI sensing pin + // @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum + // @Values: -1:Disabled, 0:A0, 1:A1, 13:A13 + // @User: Standard + GSCALAR(rssi_pin, "RSSI_PIN", -1), + + // @Param: BATT_VOLT_PIN + // @DisplayName: Battery Voltage sensing pin + // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. + // @Values: -1:Disabled, 0:A0, 1:A1, 13:A13 + // @User: Standard + GSCALAR(battery_volt_pin, "BATT_VOLT_PIN", 1), + + // @Param: BATT_CURR_PIN + // @DisplayName: Battery Current sensing pin + // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. + // @Values: -1:Disabled, 1:A1, 2:A2, 12:A12 + // @User: Standard + GSCALAR(battery_curr_pin, "BATT_CURR_PIN", 2), + + + + // @Param: SYSID_THIS_MAV + // @DisplayName: MAVLink system ID + // @Description: ID used in MAVLink protocol to identify this vehicle + // @User: Advanced GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID), + + // @Param: SYSID_MYGCS + // @DisplayName: MAVLink ground station ID + // @Description: ID used in MAVLink protocol to identify the controlling ground station + // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), // @Param: SERIAL0_BAUD @@ -42,13 +80,44 @@ const AP_Param::Info var_info[] PROGMEM = { // @Increment: 1 GSCALAR(telem_delay, "TELEM_DELAY", 0), - // @Param: MANUAL_LEVEL - // @DisplayName: Manual Level - // @Description: Setting this to Disabled(0) will enable autolevel on every boot. Setting it to Enabled(1) will do a calibration only when you tell it to. This defaults to enabled. + // @Param: MAG_ENABLED + // @DisplayName: Magnetometer (compass) enabled + // @Description: This should be set to 1 if a compass is installed + // @User: Standard // @Values: 0:Disabled,1:Enabled + GSCALAR(compass_enabled, "MAG_ENABLE", MAGNETOMETER), + + // @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: VOLT_DIVIDER + // @DisplayName: Voltage Divider + // @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(manual_level, "MANUAL_LEVEL", 1), - + 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 + // @DisplayName: Battery Capacity + // @Description: Battery capacity in milliamp-hours (mAh) + // @Units: mAh + GSCALAR(pack_capacity, "BATT_CAPACITY", HIGH_DISCHARGE), + // @Param: XTRK_GAIN_SC // @DisplayName: Crosstrack Gain // @Description: This controls how hard the Rover tries to follow the lines between waypoints, as opposed to driving directly to the next waypoint. The value is the scale between distance off the line and angle to meet the line (in Degrees * 100) @@ -66,17 +135,30 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD", XTRACK_ENTRY_ANGLE_CENTIDEGREE), - GSCALAR(command_total, "CMD_TOTAL", 0), - GSCALAR(command_index, "CMD_INDEX", 0), - - // @Param: WP_RADIUS - // @DisplayName: Waypoint radius - // @Description: The distance in meters from a waypoint when we consider the waypoint has been reached. This determines when the rover will turn along the next waypoint path. - // @Units: meters - // @Range: 0 1000 + // @Param: CRUISE_SPEED + // @DisplayName: Target speed in auto modes + // @Description: The target speed in auto missions. + // @Units: m/s + // @Range: 0 100 // @Increment: 0.1 // @User: Standard - GSCALAR(waypoint_radius, "WP_RADIUS", 2.0f), + GSCALAR(speed_cruise, "CRUISE_SPEED", 5), + + // @Param: CH7_OPTION + // @DisplayName: Channel 7 option + // @Description: What to do use channel 7 for + // @Values: 0:Nothing,1:LearnWaypoint + // @User: Standard + GSCALAR(ch7_option, "CH7_OPTION", CH7_OPTION), + + GGROUP(channel_steer, "RC1_", RC_Channel), + GGROUP(rc_2, "RC2_", RC_Channel_aux), + GGROUP(channel_throttle, "RC3_", RC_Channel), + GGROUP(rc_4, "RC4_", RC_Channel_aux), + GGROUP(rc_5, "RC5_", RC_Channel_aux), + GGROUP(rc_6, "RC6_", RC_Channel_aux), + GGROUP(rc_7, "RC7_", RC_Channel_aux), + GGROUP(rc_8, "RC8_", RC_Channel_aux), // @Param: THR_MIN // @DisplayName: Minimum Throttle @@ -96,56 +178,73 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(throttle_max, "THR_MAX", THROTTLE_MAX), - // @Param: THR_SLEWRATE - // @DisplayName: Throttle slew rate - // @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second - // @Units: Percent - // @Range: 0 100 - // @Increment: 1 - // @User: Standard - GSCALAR(throttle_slewrate, "THR_SLEWRATE", 0), - - // @Param: THR_FAILSAFE - // @DisplayName: Throttle Failsafe Enable - // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel to a low value. This can be used to detect the RC transmitter going out of range. - // @Values: 0:Disabled,1:Enabled - // @User: Standard - GSCALAR(throttle_fs_enabled, "THR_FAILSAFE", THROTTLE_FAILSAFE), - - // @Param: THR_FS_VALUE - // @DisplayName: Throttle Failsafe Value - // @Description: The PWM level on channel 3 below which throttle sailsafe triggers. - // @User: Standard - GSCALAR(throttle_fs_value, "THR_FS_VALUE", THROTTLE_FS_VALUE), - // @Param: CRUISE_THROTTLE - // @DisplayName: Throttle cruise percentage - // @Description: The target percentage of throttle to apply for auto missions. + // @DisplayName: Base throttle percentage in auto + // @Description: The base throttle percentage to use in auto mode // @Units: Percent // @Range: 0 100 // @Increment: 1 // @User: Standard GSCALAR(throttle_cruise, "CRUISE_THROTTLE", 50), - // @Param: CRUISE_SPEED - // @DisplayName: Target speed in auto modes - // @Description: The target speed in auto missions. - // @Units: m/s + // @Param: THR_SLEWRATE + // @DisplayName: Throttle slew rate + // @Description: maximum percentage change in throttle per second. A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. A value of zero means no limit. + // @Units: Percent // @Range: 0 100 - // @Increment: 0.1 + // @Increment: 1 // @User: Standard - GSCALAR(speed_cruise, "CRUISE_SPEED", 5), + GSCALAR(throttle_slewrate, "THR_SLEWRATE", 0), + + // @Param: FS_ACTION + // @DisplayName: Failsafe Action + // @Description: What to do on a failsafe event + // @Values: 0:Nothing,1:RTL,2:STOP + // @User: Standard + GSCALAR(fs_action, "FS_ACTION", 0), + + // @Param: FS_TIMEOUT + // @DisplayName: Failsafe timeout + // @Description: How long a failsafe event need to happen for before we trigger the failsafe action + // @Units: seconds + // @User: Standard + GSCALAR(fs_timeout, "FS_TIMEOUT", 10), + + // @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 to a low value. This can be used to detect the RC transmitter going out of range. + // @Values: 0:Disabled,1:Enabled + // @User: Standard + GSCALAR(fs_throttle_enabled, "FS_THR_ENABLE", 0), + + // @Param: FS_THR_VALUE + // @DisplayName: Throttle Failsafe Value + // @Description: The PWM level on channel 3 below which throttle sailsafe triggers. + // @User: Standard + GSCALAR(fs_throttle_value, "FS_THR_VALUE", 900), // @Param: FS_GCS_ENABLE // @DisplayName: GCS failsafe enable // @Description: Enable ground control station telemetry failsafe // @Values: 0:Disabled,1:Enabled // @User: Standard - GSCALAR(gcs_heartbeat_fs_enabled, "FS_GCS_ENABLE", 0), + GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE", 0), + +#if CONFIG_SONAR == ENABLED + // @Param: SONAR_ENABLE + // @DisplayName: Enable Sonar + // @Description: Setting this to Enabled(1) will enable the sonar. Setting this to Disabled(0) will disable the sonar + // @Values: 0:Disabled,1:Enabled + // @User: Standard + GSCALAR(sonar_trigger, "SONAR_TRIGGER", SONAR_TRIGGER), + GSCALAR(sonar_enabled, "SONAR_ENABLE", SONAR_ENABLED), + GSCALAR(sonar_type, "SONAR_TYPE", AP_RANGEFINDER_MAXSONARXL), +#endif + // @Param: MODE_CH // @DisplayName: Mode channel - // @Description: RC Channel to use for flight mode control + // @Description: RC Channel to use for driving mode control // @User: Advanced GSCALAR(mode_channel, "MODE_CH", MODE_CHANNEL), @@ -153,106 +252,55 @@ const AP_Param::Info var_info[] PROGMEM = { // @DisplayName: Mode1 // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided // @User: Standard - // @Description: Flight mode for switch position 1 (910 to 1230 and above 2049) + // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049) GSCALAR(mode1, "MODE1", MODE_1), // @Param: MODE2 // @DisplayName: Mode2 - // @Description: Flight mode for switch position 2 (1231 to 1360) + // @Description: Driving mode for switch position 2 (1231 to 1360) // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode2, "MODE2", MODE_2), // @Param: MODE3 // @DisplayName: Mode3 - // @Description: Flight mode for switch position 3 (1361 to 1490) + // @Description: Driving mode for switch position 3 (1361 to 1490) // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode3, "MODE3", MODE_3), // @Param: MODE4 // @DisplayName: Mode4 - // @Description: Flight mode for switch position 4 (1491 to 1620) + // @Description: Driving mode for switch position 4 (1491 to 1620) // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode4, "MODE4", MODE_4), // @Param: MODE5 // @DisplayName: Mode5 - // @Description: Flight mode for switch position 5 (1621 to 1749) + // @Description: Driving mode for switch position 5 (1621 to 1749) // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode5, "MODE5", MODE_5), // @Param: MODE6 // @DisplayName: Mode6 - // @Description: Flight mode for switch position 6 (1750 to 2049) + // @Description: Driving mode for switch position 6 (1750 to 2049) // @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided // @User: Standard GSCALAR(mode6, "MODE6", MODE_6), - GSCALAR(num_resets, "SYS_NUM_RESETS", 0), - GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), - GSCALAR(log_last_filenumber, "LOG_LASTFILE", 0), - GSCALAR(reset_switch_chan, "RST_SWITCH_CH", 0), - GSCALAR(ch7_option, "CH7_OPT", CH7_OPTION), + GSCALAR(command_total, "CMD_TOTAL", 0), + GSCALAR(command_index, "CMD_INDEX", 0), - GSCALAR(compass_enabled, "MAG_ENABLE", MAGNETOMETER), - - GSCALAR(battery_monitoring, "BATT_MONITOR", DISABLED), - GSCALAR(volt_div_ratio, "VOLT_DIVIDER", VOLT_DIV_RATIO), - GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT", CURR_AMP_PER_VOLT), - GSCALAR(input_voltage, "INPUT_VOLTS", INPUT_VOLTAGE), - GSCALAR(pack_capacity, "BATT_CAPACITY", HIGH_DISCHARGE), - - // @Param: BATT_VOLT_PIN - // @DisplayName: Battery Voltage sensing pin - // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. - // @Values: -1:Disabled, 0:A0, 1:A1, 13:A13 + // @Param: WP_RADIUS + // @DisplayName: Waypoint radius + // @Description: The distance in meters from a waypoint when we consider the waypoint has been reached. This determines when the rover will turn along the next waypoint path. + // @Units: meters + // @Range: 0 1000 + // @Increment: 0.1 // @User: Standard - GSCALAR(battery_volt_pin, "BATT_VOLT_PIN", 1), - - // @Param: BATT_CURR_PIN - // @DisplayName: Battery Current sensing pin - // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. - // @Values: -1:Disabled, 1:A1, 2:A2, 12:A12 - // @User: Standard - GSCALAR(battery_curr_pin, "BATT_CURR_PIN", 2), - - // @Param: RSSI_PIN - // @DisplayName: Receiver RSSI sensing pin - // @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum - // @Values: -1:Disabled, 0:A0, 1:A1, 13:A13 - // @User: Standard - GSCALAR(rssi_pin, "RSSI_PIN", -1), - -#if HIL_MODE != HIL_MODE_ATTITUDE -#if CONFIG_SONAR == ENABLED - // @Param: SONAR_ENABLE - // @DisplayName: Enable Sonar - // @Description: Setting this to Enabled(1) will enable the sonar. Setting this to Disabled(0) will disable the sonar - // @Values: 0:Disabled,1:Enabled - // @User: Standard - GSCALAR(sonar_enabled, "SONAR_ENABLE", SONAR_ENABLED), - GSCALAR(sonar_type, "SONAR_TYPE", AP_RANGEFINDER_MAXSONARXL), -#endif -#endif - - // ************************************************************ - // APMrover parameters - JLN update - GSCALAR(sonar_trigger, "ROV_SONAR_TRIG", SONAR_TRIGGER), - GSCALAR(booster, "ROV_BOOSTER", BOOSTER), - -// ************************************************************ - - GGROUP(channel_steer, "RC1_", RC_Channel), - GGROUP(rc_2, "RC2_", RC_Channel_aux), - GGROUP(channel_throttle, "RC3_", RC_Channel), - GGROUP(rc_4, "RC4_", RC_Channel_aux), - GGROUP(rc_5, "RC5_", RC_Channel_aux), - GGROUP(rc_6, "RC6_", RC_Channel_aux), - GGROUP(rc_7, "RC7_", RC_Channel_aux), - GGROUP(rc_8, "RC8_", RC_Channel_aux), + GSCALAR(waypoint_radius, "WP_RADIUS", 2.0f), GGROUP(pidNavSteer, "HDNG2STEER_", PID), GGROUP(pidServoSteer, "STEER2SRV_", PID), diff --git a/APMrover2/events.pde b/APMrover2/events.pde index b38f816af6..9dc3246dbd 100644 --- a/APMrover2/events.pde +++ b/APMrover2/events.pde @@ -16,9 +16,7 @@ static void failsafe_long_on_event(int fstype) case AUTO: case GUIDED: - if(g.long_fs_action == 1) { - set_mode(RTL); - } + set_mode(RTL); break; case RTL: @@ -28,15 +26,6 @@ static void failsafe_long_on_event(int fstype) gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode); } -#if BATTERY_EVENT == ENABLED -static void low_battery_event(void) -{ - gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); - set_mode(RTL); - g.throttle_cruise = THROTTLE_CRUISE; -} -#endif - static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY { if(event_repeat == 0 || (millis() - event_timer) < event_delay) diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde index 8c9ea311b2..c9da745bdd 100644 --- a/APMrover2/radio.pde +++ b/APMrover2/radio.pde @@ -83,8 +83,10 @@ static void read_radio() static void control_failsafe(uint16_t pwm) { - if(g.throttle_fs_enabled == 0) + if (!g.fs_throttle_enabled) { + // no throttle failsafe return; + } // Check for failsafe condition based on loss of GCS control if (rc_override_active) { @@ -95,8 +97,8 @@ static void control_failsafe(uint16_t pwm) } //Check for failsafe and debounce funky reads - } else if (g.throttle_fs_enabled) { - if (pwm < (unsigned)g.throttle_fs_value){ + } else if (g.fs_throttle_enabled) { + if (pwm < (unsigned)g.fs_throttle_value){ // we detect a failsafe from radio // throttle has dropped below the mark failsafeCounter++; diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index 4ad1631b5d..1f697ccfeb 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -39,10 +39,5 @@ static void read_battery(void) current_amps1 = CURRENT_AMPS(batt_curr_pin->read_average()); current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours) } - - #if BATTERY_EVENT == ENABLED - if(battery_voltage1 < LOW_VOLTAGE) low_battery_event(); - if(g.battery_monitoring == 4 && current_total1 > g.pack_capacity) low_battery_event(); - #endif } diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index 19f032bcad..4551063851 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -302,13 +302,7 @@ setup_accel_scale(uint8_t argc, const Menu::arg *argv) ins_sample_rate, flash_leds); AP_InertialSensor_UserInteractStream interact(hal.console); - bool success = ins.calibrate_accel(flash_leds, &interact); - if (success) { - if (g.manual_level == 0) { - cliSerial->println_P(PSTR("Setting MANUAL_LEVEL to 1")); - g.manual_level.set_and_save(1); - } - } + ins.calibrate_accel(flash_leds, &interact); return(0); } #endif @@ -416,16 +410,16 @@ static void report_throttle() cliSerial->printf_P(PSTR("Throttle\n")); print_divider(); - cliSerial->printf_P(PSTR("min: %d\n" - "max: %d\n" - "cruise: %d\n" - "failsafe_enabled: %d\n" - "failsafe_value: %d\n"), - (int)g.throttle_min, - (int)g.throttle_max, - (int)g.throttle_cruise, - (int)g.throttle_fs_enabled, - (int)g.throttle_fs_value); + cliSerial->printf_P(PSTR("min: %u\n" + "max: %u\n" + "cruise: %u\n" + "failsafe_enabled: %u\n" + "failsafe_value: %u\n"), + (unsigned)g.throttle_min, + (unsigned)g.throttle_max, + (unsigned)g.throttle_cruise, + (unsigned)g.fs_throttle_enabled, + (unsigned)g.fs_throttle_value); print_blanks(2); } diff --git a/APMrover2/system.pde b/APMrover2/system.pde index 0a925b941e..e9148544bd 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -383,7 +383,7 @@ static void check_long_failsafe() if(! rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) { failsafe_long_on_event(FAILSAFE_LONG); } - if(g.gcs_heartbeat_fs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { + if (g.fs_gcs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { failsafe_long_on_event(FAILSAFE_GCS); } } else { @@ -412,7 +412,7 @@ static void startup_INS_ground(bool force_accel_level) ins.init(AP_InertialSensor::COLD_START, ins_sample_rate, flash_leds); - if (force_accel_level || g.manual_level == 0) { + if (force_accel_level) { // when MANUAL_LEVEL is set to 1 we don't do accelerometer // levelling on each boot, and instead rely on the user to do // it once via the ground station diff --git a/APMrover2/test.pde b/APMrover2/test.pde index fda2052294..96e8557c7c 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -209,7 +209,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv) fail_test++; } - if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){ + if (g.fs_throttle_enabled && g.channel_throttle.get_failsafe()){ cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in); print_mode(readSwitch()); fail_test++;