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),
|
SCHED_TASK_CLASS(AC_Fence, &plane.fence, update, 10, 100, 75),
|
||||||
#endif
|
#endif
|
||||||
SCHED_TASK(read_rangefinder, 50, 100, 78),
|
SCHED_TASK(read_rangefinder, 50, 100, 78),
|
||||||
|
#if AP_ICENGINE_ENABLED
|
||||||
SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81),
|
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),
|
SCHED_TASK_CLASS(Compass, &plane.compass, cal_update, 50, 50, 84),
|
||||||
#if AP_OPTICALFLOW_ENABLED
|
#if AP_OPTICALFLOW_ENABLED
|
||||||
SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update, 50, 50, 87),
|
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;
|
return MAV_RESULT_ACCEPTED;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if AP_ICENGINE_ENABLED
|
||||||
case MAV_CMD_DO_ENGINE_CONTROL:
|
case MAV_CMD_DO_ENGINE_CONTROL:
|
||||||
if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3)) {
|
if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3)) {
|
||||||
return MAV_RESULT_FAILED;
|
return MAV_RESULT_FAILED;
|
||||||
}
|
}
|
||||||
return MAV_RESULT_ACCEPTED;
|
return MAV_RESULT_ACCEPTED;
|
||||||
|
#endif
|
||||||
|
|
||||||
#if AP_SCRIPTING_ENABLED
|
#if AP_SCRIPTING_ENABLED
|
||||||
case MAV_CMD_DO_FOLLOW:
|
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),
|
AP_SUBGROUPPTR(button_ptr, "BTN_", 1, ParametersG2, AP_Button),
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if AP_ICENGINE_ENABLED
|
||||||
// @Group: ICE_
|
// @Group: ICE_
|
||||||
// @Path: ../libraries/AP_ICEngine/AP_ICEngine.cpp
|
// @Path: ../libraries/AP_ICEngine/AP_ICEngine.cpp
|
||||||
AP_SUBGROUPINFO(ice_control, "ICE_", 2, ParametersG2, AP_ICEngine),
|
AP_SUBGROUPINFO(ice_control, "ICE_", 2, ParametersG2, AP_ICEngine),
|
||||||
|
#endif
|
||||||
|
|
||||||
// 3 was used by prototype for servo_channels
|
// 3 was used by prototype for servo_channels
|
||||||
|
|
||||||
|
@ -1240,7 +1242,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
||||||
};
|
};
|
||||||
|
|
||||||
ParametersG2::ParametersG2(void) :
|
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
|
#if HAL_SOARING_ENABLED
|
||||||
,soaring_controller(plane.TECS_controller, plane.aparm)
|
,soaring_controller(plane.TECS_controller, plane.aparm)
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -485,8 +485,10 @@ public:
|
||||||
AP_Stats stats;
|
AP_Stats stats;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if AP_ICENGINE_ENABLED
|
||||||
// internal combustion engine control
|
// internal combustion engine control
|
||||||
AP_ICEngine ice_control;
|
AP_ICEngine ice_control;
|
||||||
|
#endif
|
||||||
|
|
||||||
// RC input channels
|
// RC input channels
|
||||||
RC_Channels_Plane rc_channels;
|
RC_Channels_Plane rc_channels;
|
||||||
|
@ -559,6 +561,9 @@ public:
|
||||||
AP_Int8 man_expo_rudder;
|
AP_Int8 man_expo_rudder;
|
||||||
|
|
||||||
AP_Int32 oneshot_mask;
|
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[];
|
extern const AP_Param::Info var_info[];
|
||||||
|
|
|
@ -190,11 +190,13 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if AP_ICENGINE_ENABLED
|
||||||
case MAV_CMD_DO_ENGINE_CONTROL:
|
case MAV_CMD_DO_ENGINE_CONTROL:
|
||||||
plane.g2.ice_control.engine_control(cmd.content.do_engine_control.start_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.cold_start,
|
||||||
cmd.content.do_engine_control.height_delay_cm*0.01f);
|
cmd.content.do_engine_control.height_delay_cm*0.01f);
|
||||||
break;
|
break;
|
||||||
|
#endif
|
||||||
|
|
||||||
#if AP_SCRIPTING_ENABLED
|
#if AP_SCRIPTING_ENABLED
|
||||||
case MAV_CMD_NAV_SCRIPT_TIME:
|
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()) {
|
if (poscontrol.get_state() < QuadPlane::QPOS_LAND_FINAL && quadplane.check_land_final()) {
|
||||||
poscontrol.set_state(QuadPlane::QPOS_LAND_FINAL);
|
poscontrol.set_state(QuadPlane::QPOS_LAND_FINAL);
|
||||||
quadplane.setup_target_position();
|
quadplane.setup_target_position();
|
||||||
|
#if AP_ICENGINE_ENABLED
|
||||||
// cut IC engine if enabled
|
// cut IC engine if enabled
|
||||||
if (quadplane.land_icengine_cut != 0) {
|
if (quadplane.land_icengine_cut != 0) {
|
||||||
plane.g2.ice_control.engine_control(0, 0, 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 height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
|
||||||
float descent_rate_cms = quadplane.landing_descent_rate_cms(height_above_ground);
|
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()) {
|
if (poscontrol.get_state() == QPOS_LAND_DESCEND && check_land_final()) {
|
||||||
poscontrol.set_state(QPOS_LAND_FINAL);
|
poscontrol.set_state(QPOS_LAND_FINAL);
|
||||||
|
|
||||||
|
#if AP_ICENGINE_ENABLED
|
||||||
// cut IC engine if enabled
|
// cut IC engine if enabled
|
||||||
if (land_icengine_cut != 0) {
|
if (land_icengine_cut != 0) {
|
||||||
plane.g2.ice_control.engine_control(0, 0, 0);
|
plane.g2.ice_control.engine_control(0, 0, 0);
|
||||||
}
|
}
|
||||||
|
#endif // AP_ICENGINE_ENABLED
|
||||||
gcs().send_text(MAV_SEVERITY_INFO,"Land final started");
|
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();
|
int8_t min_throttle = aparm.throttle_min.get();
|
||||||
|
|
||||||
// apply idle governor
|
// apply idle governor
|
||||||
|
#if AP_ICENGINE_ENABLED
|
||||||
g2.ice_control.update_idle_governor(min_throttle);
|
g2.ice_control.update_idle_governor(min_throttle);
|
||||||
|
#endif
|
||||||
throttle = MAX(throttle, min_throttle);
|
throttle = MAX(throttle, min_throttle);
|
||||||
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 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 min_throttle = aparm.throttle_min.get();
|
||||||
int8_t max_throttle = aparm.throttle_max.get();
|
int8_t max_throttle = aparm.throttle_max.get();
|
||||||
|
|
||||||
|
#if AP_ICENGINE_ENABLED
|
||||||
// apply idle governor
|
// apply idle governor
|
||||||
g2.ice_control.update_idle_governor(min_throttle);
|
g2.ice_control.update_idle_governor(min_throttle);
|
||||||
|
#endif
|
||||||
|
|
||||||
if (min_throttle < 0 && !allow_reverse_thrust()) {
|
if (min_throttle < 0 && !allow_reverse_thrust()) {
|
||||||
// reverse thrust is available but inhibited.
|
// reverse thrust is available but inhibited.
|
||||||
min_throttle = 0;
|
min_throttle = 0;
|
||||||
|
@ -886,7 +890,9 @@ void Plane::set_servos(void)
|
||||||
// slew rate limit throttle
|
// slew rate limit throttle
|
||||||
throttle_slew_limit(SRV_Channel::k_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);
|
const float base_throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
|
||||||
|
#endif
|
||||||
|
|
||||||
if (!arming.is_armed()) {
|
if (!arming.is_armed()) {
|
||||||
//Some ESCs get noisy (beep error msgs) if PWM == 0.
|
//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);
|
float override_pct = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
|
||||||
if (g2.ice_control.throttle_override(override_pct, base_throttle)) {
|
if (g2.ice_control.throttle_override(override_pct, base_throttle)) {
|
||||||
// the ICE controller wants to override the throttle for starting, idle, or redline
|
// 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;
|
quadplane.vel_forward.integrator = 0;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
#endif // AP_ICENGINE_ENABLED
|
||||||
|
|
||||||
// run output mixer and send values to the hal for output
|
// run output mixer and send values to the hal for output
|
||||||
servos_output();
|
servos_output();
|
||||||
|
|
Loading…
Reference in New Issue