Browse Source

SITL: add tests for ICE Planes

apm_2208
Peter Barker 3 years ago committed by Peter Barker
parent
commit
db59117b83
  1. 3
      libraries/SITL/SIM_ICEngine.cpp
  2. 6
      libraries/SITL/SIM_ICEngine.h
  3. 3
      libraries/SITL/SIM_Plane.h

3
libraries/SITL/SIM_ICEngine.cpp

@ -30,6 +30,9 @@ float ICEngine::update(const struct sitl_input &input) @@ -30,6 +30,9 @@ float ICEngine::update(const struct sitl_input &input)
bool have_choke = choke_servo>=0;
bool have_starter = starter_servo>=0;
float throttle_demand = (input.servos[throttle_servo]-1000) * 0.001f;
if (throttle_reversed) {
throttle_demand = 1 - throttle_demand;
}
state.ignition = have_ignition?input.servos[ignition_servo]>1700:true;
state.choke = have_choke?input.servos[choke_servo]>1700:false;

6
libraries/SITL/SIM_ICEngine.h

@ -30,12 +30,13 @@ public: @@ -30,12 +30,13 @@ public:
const int8_t starter_servo;
const float slew_rate; // percent-per-second
ICEngine(uint8_t _throttle, int8_t _choke, int8_t _ignition, int8_t _starter, float _slew_rate) :
ICEngine(uint8_t _throttle, int8_t _choke, int8_t _ignition, int8_t _starter, float _slew_rate, bool _throttle_reversed) :
throttle_servo(_throttle),
choke_servo(_choke),
ignition_servo(_ignition),
starter_servo(_starter),
slew_rate(_slew_rate)
slew_rate(_slew_rate),
throttle_reversed(_throttle_reversed)
{}
// update motor state
@ -54,5 +55,6 @@ private: @@ -54,5 +55,6 @@ private:
uint8_t value;
} state, last_state;
bool overheat:1;
bool throttle_reversed;
};
}

3
libraries/SITL/SIM_Plane.h

@ -115,7 +115,8 @@ protected: @@ -115,7 +115,8 @@ protected:
choke_servo,
ignition_servo,
starter_servo,
slewrate
slewrate,
true
};
float liftCoeff(float alpha) const;

Loading…
Cancel
Save