aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_SITL: removed old servo slew rate code

Browse Source
This commit is contained in:
Andrew Tridgell 2016-10-25 19:55:09 +11:00
parent 5e03358b0b
commit d2287caf1c
2 changed files with 0 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -311,32 +311,6 @@ void SITL_State::_fdm_input_local(void)
}
#endif
/*
apply servo rate filtering
This allows simulation of servo lag
*/
void SITL_State::_apply_servo_filter(float deltat)
{
if (_sitl == nullptr || _sitl->servo_rate < 1.0f) {
// no limit
return;
}
// 1000 usec == 90 degrees
uint16_t max_change = deltat * _sitl->servo_rate * 1000 / 90;
if (max_change == 0) {
max_change = 1;
}
for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
int16_t change = (int16_t)pwm_output[i] - (int16_t)last_pwm_output[i];
if (change > max_change) {
pwm_output[i] = last_pwm_output[i] + max_change;
} else if (change < -max_change) {
pwm_output[i] = last_pwm_output[i] - max_change;
}
}
}
/*
create sitl_input structure for sending to FDM
*/
@ -359,18 +333,12 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
if (_vehicle == APMrover2) {
pwm_output[0] = pwm_output[1] = pwm_output[2] = pwm_output[3] = 1500;
}
for (i=0; i<SITL_NUM_CHANNELS; i++) {
last_pwm_output[i] = pwm_output[i];
}
}
// output at chosen framerate
uint32_t now = AP_HAL::micros();
float deltat = (now - last_update_usec) * 1.0e-6f;
last_update_usec = now;
_apply_servo_filter(deltat);
// pass wind into simulators, using a wind gradient below 60m
float altitude = _barometer?_barometer->get_altitude():0;
float wind_speed = 0;
@ -398,7 +366,6 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
} else {
input.servos[i] = pwm_output[i];
}
last_pwm_output[i] = pwm_output[i];
}
float engine_mul = _sitl?_sitl->engine_mul.get():1;

2
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -44,7 +44,6 @@ public:
int gps2_pipe(void);
ssize_t gps_read(int fd, void *buf, size_t count);
uint16_t pwm_output[SITL_NUM_CHANNELS];
uint16_t last_pwm_output[SITL_NUM_CHANNELS];
uint16_t pwm_input[SITL_RC_INPUT_CHANNELS];
bool new_rc_input;
void loop_hook(void);
@ -135,7 +134,6 @@ private:
void _output_to_flightgear(void);
void _simulator_servos(SITL::Aircraft::sitl_input &input);
void _simulator_output(bool synthetic_clock_mode);
void _apply_servo_filter(float deltat);
uint16_t _airspeed_sensor(float airspeed);
uint16_t _ground_sonar();
float _rand_float(void);