diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 4bf01743e7..ae4111338c 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -244,7 +244,7 @@ const AP_Param::Info Plane::var_info[] = { // @Units: meters // @Increment: 0.5 // @User: Advanced - GSCALAR(land_slope_recalc_shallow_threshold, "LAND_SLOPE_RCALC", 2.0f), + ASCALAR(land_slope_recalc_shallow_threshold, "LAND_SLOPE_RCALC", 2.0f), // @Param: LAND_ABORT_DEG // @DisplayName: Landing auto-abort slope threshold @@ -253,7 +253,7 @@ const AP_Param::Info Plane::var_info[] = { // @Units: degrees // @Increment: 0.1 // @User: Advanced - GSCALAR(land_slope_recalc_steep_threshold_to_abort, "LAND_ABORT_DEG", 0), + ASCALAR(land_slope_recalc_steep_threshold_to_abort, "LAND_ABORT_DEG", 0), // @Param: LAND_PITCH_CD // @DisplayName: Landing Pitch @@ -268,7 +268,7 @@ const AP_Param::Info Plane::var_info[] = { // @Units: meters // @Increment: 0.1 // @User: Advanced - GSCALAR(land_flare_alt, "LAND_FLARE_ALT", 3.0), + ASCALAR(land_flare_alt, "LAND_FLARE_ALT", 3.0), // @Param: LAND_FLARE_SEC // @DisplayName: Landing flare time @@ -285,7 +285,7 @@ const AP_Param::Info Plane::var_info[] = { // @Range: 0 30 // @Increment: 0.1 // @User: Advanced - GSCALAR(land_pre_flare_alt , "LAND_PF_ALT", 10.0), + ASCALAR(land_pre_flare_alt , "LAND_PF_ALT", 10.0), // @Param: LAND_PF_SEC // @DisplayName: Landing pre-flare time @@ -294,7 +294,7 @@ const AP_Param::Info Plane::var_info[] = { // @Range: 0 10 // @Increment: 0.1 // @User: Advanced - GSCALAR(land_pre_flare_sec , "LAND_PF_SEC", 6.0), + ASCALAR(land_pre_flare_sec , "LAND_PF_SEC", 6.0), // @Param: LAND_PF_ARSPD // @DisplayName: Landing pre-flare airspeed @@ -905,7 +905,7 @@ const AP_Param::Info Plane::var_info[] = { // @Description: Airspeed in cm/s to aim for when airspeed is enabled in auto mode. This is a calibrated (apparent) airspeed. // @Units: cm/s // @User: User - GSCALAR(airspeed_cruise_cm, "TRIM_ARSPD_CM", AIRSPEED_CRUISE_CM), + ASCALAR(airspeed_cruise_cm, "TRIM_ARSPD_CM", AIRSPEED_CRUISE_CM), // @Param: SCALING_SPEED // @DisplayName: speed used for speed scaling calculations @@ -919,7 +919,7 @@ const AP_Param::Info Plane::var_info[] = { // @Description: Minimum ground speed in cm/s when under airspeed control // @Units: cm/s // @User: Advanced - GSCALAR(min_gndspeed_cm, "MIN_GNDSPD_CM", MIN_GNDSPEED_CM), + ASCALAR(min_gndspeed_cm, "MIN_GNDSPD_CM", MIN_GNDSPEED_CM), // @Param: TRIM_PITCH_CD // @DisplayName: Pitch angle offset @@ -1077,7 +1077,7 @@ const AP_Param::Info Plane::var_info[] = { // @Description: Automatically detect a crash during AUTO flight and perform the bitmask selected action(s). Disarm will turn off motor for safety and to help against burning out ESC and motor. Setting the mode to manual will help save the servos from burning out by overexerting if the aircraft crashed in an odd orientation such as upsidedown. Set to 0 to disable crash detection. // @Bitmask: 0:Disarm // @User: Advanced - GSCALAR(crash_detection_enable, "CRASH_DETECT", 0), + ASCALAR(crash_detection_enable, "CRASH_DETECT", 0), // barometer ground calibration. The GND_ prefix is chosen for // compatibility with previous releases of ArduPlane diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 0efdcf2a5f..6798b9e16a 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -368,7 +368,6 @@ public: AP_Int8 trim_rc_at_start; AP_Int8 crash_accel_threshold; - AP_Int8 crash_detection_enable; // Feed-forward gains // @@ -472,17 +471,10 @@ public: AP_Int32 log_bitmask; AP_Int8 reset_switch_chan; AP_Int8 reset_mission_chan; - AP_Int32 airspeed_cruise_cm; AP_Int32 RTL_altitude_cm; - AP_Float land_flare_alt; AP_Int8 land_disarm_delay; AP_Int8 land_then_servos_neutral; AP_Int8 land_abort_throttle_enable; - AP_Float land_pre_flare_alt; - AP_Float land_pre_flare_sec; - AP_Float land_slope_recalc_shallow_threshold; - AP_Float land_slope_recalc_steep_threshold_to_abort; - AP_Int32 min_gndspeed_cm; AP_Int16 pitch_trim_cd; AP_Int16 FBWB_min_altitude_cm; AP_Int8 hil_servos; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 6d53e8b2c1..318e0f981f 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -837,13 +837,13 @@ void Plane::do_change_speed(const AP_Mission::Mission_Command& cmd) { case 0: // Airspeed if (cmd.content.speed.target_ms > 0) { - g.airspeed_cruise_cm.set(cmd.content.speed.target_ms * 100); + aparm.airspeed_cruise_cm.set(cmd.content.speed.target_ms * 100); gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms); } break; case 1: // Ground speed gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)cmd.content.speed.target_ms); - g.min_gndspeed_cm.set(cmd.content.speed.target_ms * 100); + aparm.min_gndspeed_cm.set(cmd.content.speed.target_ms * 100); break; } diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index c01edc7115..e82ac3010d 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -18,7 +18,7 @@ void Plane::update_is_flying_5Hz(void) bool is_flying_bool; uint32_t now_ms = AP_HAL::millis(); - uint32_t ground_speed_thresh_cm = (g.min_gndspeed_cm > 0) ? ((uint32_t)(g.min_gndspeed_cm*0.9f)) : GPS_IS_FLYING_SPEED_CMS; + uint32_t ground_speed_thresh_cm = (aparm.min_gndspeed_cm > 0) ? ((uint32_t)(aparm.min_gndspeed_cm*0.9f)) : GPS_IS_FLYING_SPEED_CMS; bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) && (gps.ground_speed_cm() >= ground_speed_thresh_cm); @@ -189,7 +189,7 @@ bool Plane::is_flying(void) */ void Plane::crash_detection_update(void) { - if (control_mode != AUTO || !g.crash_detection_enable) + if (control_mode != AUTO || !aparm.crash_detection_enable) { // crash detection is only available in AUTO mode crash_state.debounce_timer_ms = 0; @@ -284,7 +284,7 @@ void Plane::crash_detection_update(void) } else if ((now_ms - crash_state.debounce_timer_ms >= crash_state.debounce_time_total_ms) && !crash_state.is_crashed) { crash_state.is_crashed = true; - if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { + if (aparm.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { if (crashed_near_land_waypoint) { gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken"); } else { @@ -292,7 +292,7 @@ void Plane::crash_detection_update(void) } } else { - if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { + if (aparm.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { disarm_motors(); } landing.complete = true; diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index 95512e9212..e77d9d6e31 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -62,9 +62,9 @@ bool Plane::verify_land() bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE); - bool below_flare_alt = (height <= g.land_flare_alt); + bool below_flare_alt = (height <= aparm.land_flare_alt); bool below_flare_sec = (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec); - bool probably_crashed = (g.crash_detection_enable && fabsf(auto_state.sink_rate) < 0.2f && !is_flying()); + bool probably_crashed = (aparm.crash_detection_enable && fabsf(auto_state.sink_rate) < 0.2f && !is_flying()); if ((on_approach_stage && below_flare_alt) || (on_approach_stage && below_flare_sec && (auto_state.wp_proportion > 0.5)) || @@ -91,13 +91,13 @@ bool Plane::verify_land() // been set for landing. We don't do this till ground // speed drops below 3.0 m/s as otherwise we will change // target speeds too early. - g.airspeed_cruise_cm.load(); - g.min_gndspeed_cm.load(); + aparm.airspeed_cruise_cm.load(); + aparm.min_gndspeed_cm.load(); aparm.throttle_cruise.load(); } } else if (!landing.complete && !landing.pre_flare && aparm.land_pre_flare_airspeed > 0) { - bool reached_pre_flare_alt = g.land_pre_flare_alt > 0 && (height <= g.land_pre_flare_alt); - bool reached_pre_flare_sec = g.land_pre_flare_sec > 0 && (height <= auto_state.sink_rate * g.land_pre_flare_sec); + bool reached_pre_flare_alt = aparm.land_pre_flare_alt > 0 && (height <= aparm.land_pre_flare_alt); + bool reached_pre_flare_sec = aparm.land_pre_flare_sec > 0 && (height <= auto_state.sink_rate * aparm.land_pre_flare_sec); if (reached_pre_flare_alt || reached_pre_flare_sec) { landing.pre_flare = true; update_flight_stage(); @@ -142,8 +142,8 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void) // altitude and moving the prev_wp to that location. From there float correction_delta = fabsf(rangefinder_state.last_stable_correction) - fabsf(rangefinder_state.correction); - if (g.land_slope_recalc_shallow_threshold <= 0 || - fabsf(correction_delta) < g.land_slope_recalc_shallow_threshold) { + if (aparm.land_slope_recalc_shallow_threshold <= 0 || + fabsf(correction_delta) < aparm.land_slope_recalc_shallow_threshold) { return; } @@ -161,7 +161,7 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void) // correction positive means we're too low so we should continue on with // the newly computed shallower slope instead of pitching/throttling up - } else if (g.land_slope_recalc_steep_threshold_to_abort > 0) { + } else if (aparm.land_slope_recalc_steep_threshold_to_abort > 0) { // correction negative means we're too high and need to point down (and speed up) to re-align // to land on target. A large negative correction means we would have to dive down a lot and will // generating way too much speed that we can not bleed off in time. It is better to remember @@ -173,13 +173,13 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void) float initial_slope_deg = degrees(atan(landing.initial_slope)); // is projected slope too steep? - if (new_slope_deg - initial_slope_deg > g.land_slope_recalc_steep_threshold_to_abort) { + if (new_slope_deg - initial_slope_deg > aparm.land_slope_recalc_steep_threshold_to_abort) { GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)", (double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg)); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!"); landing.alt_offset = rangefinder_state.correction; auto_state.commanded_go_around = 1; - g.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once + aparm.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once } } } @@ -223,12 +223,12 @@ void Plane::setup_landing_glide_slope(void) // the height we aim for is the one to give us the right flare point float aim_height = aparm.land_flare_sec * sink_rate; if (aim_height <= 0) { - aim_height = g.land_flare_alt; + aim_height = aparm.land_flare_alt; } // don't allow the aim height to be too far above LAND_FLARE_ALT - if (g.land_flare_alt > 0 && aim_height > g.land_flare_alt*2) { - aim_height = g.land_flare_alt*2; + if (aparm.land_flare_alt > 0 && aim_height > aparm.land_flare_alt*2) { + aim_height = aparm.land_flare_alt*2; } // calculate slope to landing point diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 581e24524a..38f46511e8 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -85,7 +85,7 @@ void Plane::calc_airspeed_errors() float airspeed_measured_cm = airspeed.get_airspeed_cm(); // Normal airspeed target - target_airspeed_cm = g.airspeed_cruise_cm; + target_airspeed_cm = aparm.airspeed_cruise_cm; // FBW_B airspeed target if (control_mode == FLY_BY_WIRE_B || @@ -124,7 +124,7 @@ void Plane::calc_airspeed_errors() // Set target to current airspeed + ground speed undershoot, // but only when this is faster than the target airspeed commanded // above. - if (control_mode >= FLY_BY_WIRE_B && (g.min_gndspeed_cm > 0)) { + if (control_mode >= FLY_BY_WIRE_B && (aparm.min_gndspeed_cm > 0)) { int32_t min_gnd_target_airspeed = airspeed_measured_cm + groundspeed_undershoot; if (min_gnd_target_airspeed > target_airspeed_cm) target_airspeed_cm = min_gnd_target_airspeed; @@ -154,7 +154,7 @@ void Plane::calc_gndspeed_undershoot() Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x); yawVect.normalize(); float gndSpdFwd = yawVect * gndVel; - groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - gndSpdFwd*100) : 0; + groundspeed_undershoot = (aparm.min_gndspeed_cm > 0) ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0; } } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 3baa107cb9..ceaaa44558 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1822,7 +1822,7 @@ void QuadPlane::check_land_complete(void) poscontrol.state = QPOS_LAND_COMPLETE; plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete"); // reload target airspeed which could have been modified by the mission - plane.g.airspeed_cruise_cm.load(); + plane.aparm.airspeed_cruise_cm.load(); } /* diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index e0e5a7dd3a..0e6cb77ade 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -220,7 +220,7 @@ void Plane::read_radio() if (g.throttle_nudge && channel_throttle->get_servo_out() > 50 && geofence_stickmixing()) { float nudge = (channel_throttle->get_servo_out() - 50) * 0.02f; if (ahrs.airspeed_sensor_enabled()) { - airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge; + airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge; } else { throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge; } diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 8693b272c3..861e5094a5 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -862,7 +862,7 @@ bool Plane::disarm_motors(void) change_arm_state(); // reload target airspeed which could have been modified by a mission - plane.g.airspeed_cruise_cm.load(); + plane.aparm.airspeed_cruise_cm.load(); return true; } diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index b68cee7189..50b805b99d 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -139,7 +139,7 @@ void Plane::takeoff_calc_pitch(void) nav_pitch_cd = takeoff_pitch_min_cd; } } else { - nav_pitch_cd = ((gps.ground_speed()*100) / (float)g.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd; + nav_pitch_cd = ((gps.ground_speed()*100) / (float)aparm.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd; nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd); } } diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 0f4e379fd2..c029c393eb 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -35,13 +35,21 @@ public: AP_Int8 takeoff_throttle_max; AP_Int16 airspeed_min; AP_Int16 airspeed_max; + AP_Int32 airspeed_cruise_cm; + AP_Int32 min_gndspeed_cm; + AP_Int8 crash_detection_enable; AP_Int16 roll_limit_cd; AP_Int16 pitch_limit_max_cd; AP_Int16 pitch_limit_min_cd; AP_Int8 autotune_level; AP_Int16 land_pitch_cd; + AP_Float land_flare_alt; AP_Float land_flare_sec; AP_Float land_pre_flare_airspeed; + AP_Float land_pre_flare_alt; + AP_Float land_pre_flare_sec; + AP_Float land_slope_recalc_shallow_threshold; + AP_Float land_slope_recalc_steep_threshold_to_abort; AP_Int8 stall_prevention; struct Rangefinder_State {