AP_Motors: throttle_pass_through accepts pwm

This commit is contained in:
Jason Short 2014-09-19 22:06:29 +09:00 committed by Randy Mackay
parent 1c11a91ec2
commit b57539a9ad
3 changed files with 8 additions and 6 deletions

View File

@ -147,7 +147,7 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan)
read_radio();
// pass through throttle to motors
motors.throttle_pass_through();
motors.throttle_pass_through(g.rc_3.radio_in);
// read some compass values
compass.read();

View File

@ -117,14 +117,15 @@ void AP_Motors::set_mid_throttle(uint16_t mid_throttle)
_hover_out = _rc_throttle.radio_min + (float)(_rc_throttle.radio_max - _rc_throttle.radio_min) * mid_throttle / 1000.0f;
}
// throttle_pass_through - passes pilot's throttle input directly to all motors - dangerous but used for initialising ESCs
void AP_Motors::throttle_pass_through()
// throttle_pass_through - passes provided pwm directly to all motors - dangerous but used for initialising ESCs
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_Motors::throttle_pass_through(int16_t pwm)
{
if (armed()) {
// send the pilot's input directly to each enabled motor
for (int16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), _rc_throttle.radio_in);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), pwm);
}
}
}

View File

@ -117,8 +117,9 @@ public:
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
virtual void output_test(uint8_t motor_seq, int16_t pwm) = 0;
// throttle_pass_through - passes pilot's throttle input directly to all motors - dangerous but used for initialising ESCs
virtual void throttle_pass_through();
// throttle_pass_through - passes provided pwm directly to all motors - dangerous but used for initialising ESCs
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
virtual void throttle_pass_through(int16_t pwm);
// setup_throttle_curve - used to linearlise thrust output by motors
// returns true if curve is created successfully