|
|
|
@ -749,10 +749,9 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
@@ -749,10 +749,9 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
|
|
|
|
|
/* this maps the registers used for PWM outputs. The RC
|
|
|
|
|
* driver updates these whenever it wants the channel output |
|
|
|
|
* to change */ |
|
|
|
|
uint8_t i; |
|
|
|
|
|
|
|
|
|
if (last_update_usec == 0 || !output_ready) { |
|
|
|
|
for (i=0; i<SITL_NUM_CHANNELS; i++) { |
|
|
|
|
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) { |
|
|
|
|
pwm_output[i] = 1000; |
|
|
|
|
} |
|
|
|
|
if (_vehicle == ArduPlane) { |
|
|
|
@ -807,7 +806,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
@@ -807,7 +806,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
|
|
|
|
|
input.wind.turbulence = _sitl?_sitl->wind_turbulance:0; |
|
|
|
|
input.wind.dir_z = wind_dir_z; |
|
|
|
|
|
|
|
|
|
for (i=0; i<SITL_NUM_CHANNELS; i++) { |
|
|
|
|
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) { |
|
|
|
|
if (pwm_output[i] == 0xFFFF) { |
|
|
|
|
input.servos[i] = 0; |
|
|
|
|
} else { |
|
|
|
@ -834,7 +833,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
@@ -834,7 +833,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
|
|
|
|
|
// do a little quadplane dance
|
|
|
|
|
float hover_throttle = 0.0f; |
|
|
|
|
uint8_t running_motors = 0; |
|
|
|
|
for (i=0; i < sitl_model->get_num_motors() - 1; i++) { |
|
|
|
|
for (uint8_t i=0; i < sitl_model->get_num_motors() - 1; i++) { |
|
|
|
|
float motor_throttle = constrain_float((input.servos[sitl_model->get_motors_offset() + i] - 1000) / 1000.0f, 0.0f, 1.0f); |
|
|
|
|
// update motor_on flag
|
|
|
|
|
if (!is_zero(motor_throttle)) { |
|
|
|
@ -857,7 +856,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
@@ -857,7 +856,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
|
|
|
|
|
} else { |
|
|
|
|
// run checks on each motor
|
|
|
|
|
uint8_t running_motors = 0; |
|
|
|
|
for (i=0; i < sitl_model->get_num_motors(); i++) { |
|
|
|
|
for (uint8_t i=0; i < sitl_model->get_num_motors(); i++) { |
|
|
|
|
float motor_throttle = constrain_float((input.servos[i] - 1000) / 1000.0f, 0.0f, 1.0f); |
|
|
|
|
// update motor_on flag
|
|
|
|
|
if (!is_zero(motor_throttle)) { |
|
|
|
@ -880,7 +879,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
@@ -880,7 +879,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
|
|
|
|
|
if (_sitl->state.battery_voltage <= 0) { |
|
|
|
|
if (_vehicle == ArduSub) { |
|
|
|
|
voltage = _sitl->batt_voltage; |
|
|
|
|
for (i = 0; i < 6; i++) { |
|
|
|
|
for (uint8_t i=0; i<6; i++) { |
|
|
|
|
float pwm = input.servos[i]; |
|
|
|
|
//printf("i: %d, pwm: %.2f\n", i, pwm);
|
|
|
|
|
float fraction = fabsf((pwm - 1500) / 500.0f); |
|
|
|
|