// Called at 40 Hz, before sending servo pulses
void dcm_servo_callback_prepare_outputs(void)
{
if (!dcm_flags._.calib_finished)
{
udb_pwOut[ROLL_OUTPUT_CHANNEL] = 3000;
udb_pwOut[PITCH_OUTPUT_CHANNEL] = 3000;
udb_pwOut[YAW_OUTPUT_CHANNEL] = 3000;
}
else
{
union longww accum;
accum.WW = __builtin_mulss(rmat[6], 4000);
udb_pwOut[ROLL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(rmat[7], 4000);
udb_pwOut[PITCH_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(rmat[1], 4000);
udb_pwOut[YAW_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
}
// Serial output at 10Hz
if ((udb_heartbeat_counter % (HEARTBEAT_HZ/10)) == 0)
{
if (dcm_flags._.calib_finished)
{
send_debug_line();
}
}
}
void hil_rc_input_adjust(char *inChannelName, int inChannelIndex, int delta)
{
udb_pwIn[inChannelIndex] = udb_servo_pulsesat(udb_pwIn[inChannelIndex] + delta);
if (inChannelIndex == THROTTLE_INPUT_CHANNEL) {
printf("%s = %d%%\n", inChannelName, (udb_pwIn[inChannelIndex]-udb_pwTrim[inChannelIndex])/20);
}
else {
printf("%s = %d%%\n", inChannelName, (udb_pwIn[inChannelIndex]-udb_pwTrim[inChannelIndex])/10);
}
}
// Called at HEARTBEAT_HZ, before sending servo pulses
void udb_heartbeat_callback(void)
{
if (calib_countdown) {
udb_pwOut[ROLL_OUTPUT_CHANNEL] = 3000;
udb_pwOut[PITCH_OUTPUT_CHANNEL] = 3000;
udb_pwOut[YAW_OUTPUT_CHANNEL] = 3000;
udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] = 3000;
udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] = 3000;
udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] = 3000;
} else if (eepromSuccess == 0 && eepromFailureFlashCount) {
// eeprom failure!
DPRINT("eeprom failure!\r\n");
if (udb_heartbeat_counter % 6 == 0) {
udb_led_toggle(LED_RED);
udb_led_toggle(LED_GREEN);
udb_led_toggle(LED_BLUE);
udb_led_toggle(LED_ORANGE);
eepromFailureFlashCount--;
}
} else {
union longww accum;
accum.WW = __builtin_mulss(y_rate, 4000);
udb_pwOut[ROLL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(x_rate, 4000);
udb_pwOut[PITCH_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(z_rate, 4000);
udb_pwOut[YAW_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(x_accel, 4000);
udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(y_accel, 4000);
udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
accum.WW = __builtin_mulss(z_accel, 4000);
udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] = udb_servo_pulsesat(3000 + accum._.W1);
if ((udb_heartbeat_counter / 600) % 2 == 0) {
led_on(LED_RED);
((abs(udb_pwOut[ROLL_OUTPUT_CHANNEL] - 3000) > RATE_THRESHOLD_LED) ? led_on(LED_ORANGE) : led_off(LED_ORANGE));
((abs(udb_pwOut[PITCH_OUTPUT_CHANNEL] - 3000) > RATE_THRESHOLD_LED) ? led_on(LED_BLUE) : led_off(LED_BLUE));
((abs(udb_pwOut[YAW_OUTPUT_CHANNEL] - 3000) > RATE_THRESHOLD_LED) ? led_on(LED_GREEN) : led_off(LED_GREEN));
} else {
led_off(LED_RED);
((abs(udb_pwOut[X_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? led_on(LED_ORANGE) : led_off(LED_ORANGE));
((abs(udb_pwOut[Y_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? led_on(LED_BLUE) : led_off(LED_BLUE));
((abs(udb_pwOut[Z_ACCEL_OUTPUT_CHANNEL] - 3000) > ACCEL_THRESHOLD_LED) ? led_on(LED_GREEN) : led_off(LED_GREEN));
}
}
}
// Apply boosts if in a stabilized mode
if (udb_flags._.radio_on && flags._.pitch_feedback)
{
pwManual[AILERON_INPUT_CHANNEL] += ((pwManual[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]) * aileronbgain) >> 3;
pwManual[ELEVATOR_INPUT_CHANNEL] += ((pwManual[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]) * elevatorbgain) >> 3;
pwManual[RUDDER_INPUT_CHANNEL] += ((pwManual[RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL]) * rudderbgain) >> 3;
}
// Standard airplane airframe
// Mix roll_control into ailerons
// Mix pitch_control into elevators
// Mix yaw control and waggle into rudder
#if (AIRFRAME_TYPE == AIRFRAME_STANDARD)
temp = pwManual[AILERON_INPUT_CHANNEL] + REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_control + waggle);
udb_pwOut[AILERON_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
udb_pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] = 3000 +
REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED, udb_pwOut[AILERON_OUTPUT_CHANNEL] - 3000);
temp = pwManual[ELEVATOR_INPUT_CHANNEL] + REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control);
udb_pwOut[ELEVATOR_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
temp = pwManual[RUDDER_INPUT_CHANNEL] + REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control - waggle);
udb_pwOut[RUDDER_OUTPUT_CHANNEL] = udb_servo_pulsesat(temp);
if (pwManual[THROTTLE_INPUT_CHANNEL] == 0)
{
udb_pwOut[THROTTLE_OUTPUT_CHANNEL] = 0;
}
else
