From b0bee613d436077653d4986e3139950c4942f2bb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 15 Jul 2022 21:50:12 +1000 Subject: [PATCH] ArduPlane: add and use AP_ICENGINE_ENABLE --- ArduPlane/ArduPlane.cpp | 2 ++ ArduPlane/GCS_Mavlink.cpp | 2 ++ ArduPlane/Parameters.cpp | 7 ++++++- ArduPlane/Parameters.h | 5 +++++ ArduPlane/commands_logic.cpp | 2 ++ ArduPlane/mode_qloiter.cpp | 2 ++ ArduPlane/quadplane.cpp | 2 ++ ArduPlane/servos.cpp | 10 +++++++++- 8 files changed, 30 insertions(+), 2 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 3773acdc6d..a627ae17e1 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -82,7 +82,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK_CLASS(AC_Fence, &plane.fence, update, 10, 100, 75), #endif SCHED_TASK(read_rangefinder, 50, 100, 78), +#if AP_ICENGINE_ENABLED SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81), +#endif SCHED_TASK_CLASS(Compass, &plane.compass, cal_update, 50, 50, 84), #if AP_OPTICALFLOW_ENABLED SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update, 50, 50, 87), diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 51cf09cbeb..d50062ddfa 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1097,11 +1097,13 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l return MAV_RESULT_ACCEPTED; #endif +#if AP_ICENGINE_ENABLED case MAV_CMD_DO_ENGINE_CONTROL: if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; +#endif #if AP_SCRIPTING_ENABLED case MAV_CMD_DO_FOLLOW: diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 584c0354a0..a06edc543c 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1016,9 +1016,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPPTR(button_ptr, "BTN_", 1, ParametersG2, AP_Button), #endif +#if AP_ICENGINE_ENABLED // @Group: ICE_ // @Path: ../libraries/AP_ICEngine/AP_ICEngine.cpp AP_SUBGROUPINFO(ice_control, "ICE_", 2, ParametersG2, AP_ICEngine), +#endif // 3 was used by prototype for servo_channels @@ -1240,7 +1242,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { }; ParametersG2::ParametersG2(void) : - ice_control(plane.rpm_sensor) + unused_integer{1} +#if AP_ICENGINE_ENABLED + ,ice_control(plane.rpm_sensor) +#endif #if HAL_SOARING_ENABLED ,soaring_controller(plane.TECS_controller, plane.aparm) #endif diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 9334c87d5c..cf32065386 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -485,8 +485,10 @@ public: AP_Stats stats; #endif +#if AP_ICENGINE_ENABLED // internal combustion engine control AP_ICEngine ice_control; +#endif // RC input channels RC_Channels_Plane rc_channels; @@ -559,6 +561,9 @@ public: AP_Int8 man_expo_rudder; AP_Int32 oneshot_mask; + + // just to make compilation easier when all things are compiled out... + uint8_t unused_integer; }; extern const AP_Param::Info var_info[]; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index a7c2041eae..0a5129d2ce 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -190,11 +190,13 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) break; #endif +#if AP_ICENGINE_ENABLED case MAV_CMD_DO_ENGINE_CONTROL: plane.g2.ice_control.engine_control(cmd.content.do_engine_control.start_control, cmd.content.do_engine_control.cold_start, cmd.content.do_engine_control.height_delay_cm*0.01f); break; +#endif #if AP_SCRIPTING_ENABLED case MAV_CMD_NAV_SCRIPT_TIME: diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 0a7a1804e8..7f480ae337 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -89,10 +89,12 @@ void ModeQLoiter::run() if (poscontrol.get_state() < QuadPlane::QPOS_LAND_FINAL && quadplane.check_land_final()) { poscontrol.set_state(QuadPlane::QPOS_LAND_FINAL); quadplane.setup_target_position(); +#if AP_ICENGINE_ENABLED // cut IC engine if enabled if (quadplane.land_icengine_cut != 0) { plane.g2.ice_control.engine_control(0, 0, 0); } +#endif // AP_ICENGINE_ENABLED } float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); float descent_rate_cms = quadplane.landing_descent_rate_cms(height_above_ground); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 55f9617cad..7046eb0add 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3295,10 +3295,12 @@ bool QuadPlane::verify_vtol_land(void) if (poscontrol.get_state() == QPOS_LAND_DESCEND && check_land_final()) { poscontrol.set_state(QPOS_LAND_FINAL); +#if AP_ICENGINE_ENABLED // cut IC engine if enabled if (land_icengine_cut != 0) { plane.g2.ice_control.engine_control(0, 0, 0); } +#endif // AP_ICENGINE_ENABLED gcs().send_text(MAV_SEVERITY_INFO,"Land final started"); } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 0bd8e26f56..fb49f2facf 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -394,7 +394,9 @@ void Plane::set_servos_manual_passthrough(void) int8_t min_throttle = aparm.throttle_min.get(); // apply idle governor +#if AP_ICENGINE_ENABLED g2.ice_control.update_idle_governor(min_throttle); +#endif throttle = MAX(throttle, min_throttle); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); } @@ -499,9 +501,11 @@ void Plane::set_servos_controlled(void) int8_t min_throttle = aparm.throttle_min.get(); int8_t max_throttle = aparm.throttle_max.get(); +#if AP_ICENGINE_ENABLED // apply idle governor g2.ice_control.update_idle_governor(min_throttle); - +#endif + if (min_throttle < 0 && !allow_reverse_thrust()) { // reverse thrust is available but inhibited. min_throttle = 0; @@ -886,7 +890,9 @@ void Plane::set_servos(void) // slew rate limit throttle throttle_slew_limit(SRV_Channel::k_throttle); +#if AP_ICENGINE_ENABLED const float base_throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); +#endif if (!arming.is_armed()) { //Some ESCs get noisy (beep error msgs) if PWM == 0. @@ -913,6 +919,7 @@ void Plane::set_servos(void) } } +#if AP_ICENGINE_ENABLED float override_pct = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); if (g2.ice_control.throttle_override(override_pct, base_throttle)) { // the ICE controller wants to override the throttle for starting, idle, or redline @@ -921,6 +928,7 @@ void Plane::set_servos(void) quadplane.vel_forward.integrator = 0; #endif } +#endif // AP_ICENGINE_ENABLED // run output mixer and send values to the hal for output servos_output();