// 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