diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 19b1a69ed1..0bd28534ad 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -60,6 +60,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(barometer_accumulate, 50, 150), SCHED_TASK(update_notify, 50, 300), SCHED_TASK(read_rangefinder, 50, 100), + SCHED_TASK(ice_update, 10, 100), SCHED_TASK(compass_cal_update, 50, 50), SCHED_TASK(accel_cal_update, 10, 50), #if OPTFLOW == ENABLED diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 6f8fef929a..8e2169abac 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -1256,6 +1256,13 @@ void Plane::set_servos(void) } } + uint8_t override_pct; + if (g2.ice_control.throttle_override(override_pct)) { + // the ICE controller wants to override the throttle for starting + channel_throttle->set_servo_out(override_pct); + channel_throttle->calc_pwm(); + } + // send values to the PWM timers for output // ---------------------------------------- if (g.rudder_only == 0) { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 6b200bac32..35614217d0 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1677,6 +1677,14 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_ACCEPTED; } break; + + case MAV_CMD_DO_ENGINE_CONTROL: + if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3)) { + result = MAV_RESULT_FAILED; + } else { + result = MAV_RESULT_ACCEPTED; + } + break; default: break; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 728561f6b2..170d2b71be 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1373,9 +1373,19 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/AP_Button/AP_Button.cpp AP_SUBGROUPINFO(button, "BTN_", 1, ParametersG2, AP_Button), + // @Group: ICE_ + // @Path: ../libraries/AP_ICEngine/AP_ICEngine.cpp + AP_SUBGROUPINFO(ice_control, "ICE_", 2, ParametersG2, AP_ICEngine), + AP_GROUPEND }; +ParametersG2::ParametersG2(void) : + ice_control(plane.rpm_sensor, plane.ahrs) +{ + AP_Param::setup_object_defaults(this, var_info); +} + /* This is a conversion table from old parameter values to new parameter names. The startup code looks for saved values of the old diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index f2050593a2..14f4b762c7 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -574,13 +574,16 @@ public: */ class ParametersG2 { public: - ParametersG2(void) { AP_Param::setup_object_defaults(this, var_info); } + ParametersG2(void); // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; // button reporting library AP_Button button; + + // internal combustion engine control + AP_ICEngine ice_control; }; extern const AP_Param::Info var_info[]; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1a13db5794..2a5e5ac0e8 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -89,6 +89,7 @@ #include #include #include +#include #include "GCS_Mavlink.h" #include "quadplane.h" @@ -132,6 +133,7 @@ class Plane : public AP_HAL::HAL::Callbacks { public: friend class GCS_MAVLINK_Plane; friend class Parameters; + friend class ParametersG2; friend class AP_Arming_Plane; friend class QuadPlane; friend class AP_Tuning_Plane; @@ -956,6 +958,7 @@ private: void read_receiver_rssi(void); void rpm_update(void); void button_update(void); + void ice_update(void); void report_radio(); void report_ins(); void report_compass(); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 287b94e627..bbca724fb6 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -225,6 +225,12 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_VTOL_TRANSITION: plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)cmd.content.do_vtol_transition.target_state); break; + + case MAV_CMD_DO_ENGINE_CONTROL: + plane.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; } return true; diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index ef445520f6..e6475eb7b8 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -164,3 +164,11 @@ void Plane::button_update(void) { g2.button.update(); } + +/* + update AP_ICEngine + */ +void Plane::ice_update(void) +{ + g2.ice_control.update(); +}