mirror of https://github.com/ArduPilot/ardupilot
ArduPlane: add and use AP_ICENGINE_ENABLE
This commit is contained in:
parent
8af84cf236
commit
b0bee613d4
|
@ -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),
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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[];
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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");
|
||||
}
|
||||
|
||||
|
|
|
@ -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();
|
||||
|
|
Loading…
Reference in New Issue