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

AP_MotorsMatrix: implement output_to_motors

Browse Source
This commit is contained in:
Leonard Hall 2016-01-18 14:18:34 +09:00 committed by Randy Mackay
parent cdec8f3387
commit 397940a9a4
2 changed files with 48 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -98,6 +98,51 @@ void AP_MotorsMatrix::output_min()
hal.rcout->push(); hal.rcout->push();
} }
void AP_MotorsMatrix::output_to_motors()
{
int8_t i;
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final pwm values sent to the motor
switch (_multicopter_flags.spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
// set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = _throttle_radio_min;
}
}
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle);
}
}
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = calc_thrust_to_pwm(_thrust_rpyt_out[i]);
}
}
break;
}
// send output to each motor
hal.rcout->cork();
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, motor_out[i]);
}
}
hal.rcout->push();
}
// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used) // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsMatrix::get_motor_mask() uint16_t AP_MotorsMatrix::get_motor_mask()

3
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -42,6 +42,9 @@ public:
// output_min - sends minimum values out to the motors // output_min - sends minimum values out to the motors
virtual void output_min(); virtual void output_min();
// output_to_motors - sends minimum values out to the motors
virtual void output_to_motors();
// add_motor using just position and yaw_factor (or prop direction) // add_motor using just position and yaw_factor (or prop direction)
void add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order); void add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order);