// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed()
{
if (_servo_test_cycle_counter > 0){
// perform boot-up servo test cycle if enabled
servo_test();
} else {
// manual override (i.e. when setting up swash)
switch (_servo_mode) {
case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH:
// pass pilot commands straight through to swash
_roll_control_input = _roll_radio_passthrough;
_pitch_control_input = _pitch_radio_passthrough;
_throttle_control_input = _throttle_radio_passthrough;
_yaw_control_input = _yaw_radio_passthrough;
break;
case SERVO_CONTROL_MODE_MANUAL_CENTER:
// fixate mid collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = _collective_mid_pwm;
_yaw_control_input = 0;
break;
case SERVO_CONTROL_MODE_MANUAL_MAX:
// fixate max collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = 1000;
_yaw_control_input = 4500;
break;
case SERVO_CONTROL_MODE_MANUAL_MIN:
// fixate min collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = 0;
_yaw_control_input = -4500;
break;
case SERVO_CONTROL_MODE_MANUAL_OSCILLATE:
// use servo_test function from child classes
servo_test();
break;
default:
// no manual override
break;
}
}
// ensure swash servo endpoints haven't been moved
init_outputs();
// continuously recalculate scalars to allow setup
calculate_scalars();
// helicopters always run stabilizing flight controls
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_STOP);
}
int main(void)
{
usart0_init(25, 8, 1, USART_PARITY_EVEN,1); //38400
usart1_init(25, 8, 1, USART_PARITY_DISABLED,0); //TODO check oi_alternative OI_ALTERNATE_BAUD_RATE
_delay_ms(333);
oi_switch_baud_rate();
_delay_ms(333);
oi_init();
oi_full_mode();
int i;
int val;
for (i = 0; i < 10; i++)
{
val = i % 2;
oi_set_leds(val, val, val, val, 0xFF * val, 0xFF);
_delay_ms(50);
}
oi_init();
//input_capture_test();
printf0("Hello world!\r\n");
printf0(" 0x%02X 0x%02X 0x%02X", UCSR0A, UCSR0B, UCSR0C);
_delay_ms(3000);
doSweepLoop();
doPingLoop();
doIrLoop();
servo_test();
}
