| /branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.c |
|---|
| 2,6 → 2,7 |
| #include <stddef.h> |
| #include <string.h> |
| #include "configuration.h" |
| #include "controlMixer.h" |
| #include "rc.h" |
| #include "output.h" |
| #include "flight.h" |
| 34,8 → 35,6 |
| {offsetof(ParamSet_t, gyroPID[2].I), offsetof(DynamicParams_t, gyroPID[2].I),0,255}, |
| {offsetof(ParamSet_t, gyroPID[2].D), offsetof(DynamicParams_t, gyroPID[2].D),0,255}, |
| {offsetof(ParamSet_t, gimbalServoConfigurations[0].manualControl), offsetof(DynamicParams_t, gimbalServoManualControl[0]),0,255}, |
| {offsetof(ParamSet_t, gimbalServoConfigurations[1].manualControl), offsetof(DynamicParams_t, gimbalServoManualControl[1]),0,255}, |
| {offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255}, |
| {offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255}, |
| }; |
| 90,17 → 89,10 |
| // Servos |
| staticParams.servoCount = 7; |
| staticParams.servosReverse = CONTROL_SERVO_REVERSE_AILERONS; |
| staticParams.servos[CONTROL_ELEVATOR].reverse = 0; |
| staticParams.servos[CONTROL_AILERONS].reverse = 1; |
| staticParams.servos[CONTROL_RUDDER].reverse = 0; |
| staticParams.gimbalServoMaxManualSpeed = 10; |
| for (uint8_t i=0; i<2; i++) { |
| staticParams.gimbalServoConfigurations[i].manualControl = 128; |
| staticParams.gimbalServoConfigurations[i].stabilizationFactor = 0; |
| staticParams.gimbalServoConfigurations[i].minValue = 32; |
| staticParams.gimbalServoConfigurations[i].maxValue = 224; |
| staticParams.gimbalServoConfigurations[i].flags = 0; |
| } |
| // Battery warning and emergency flight |
| staticParams.batteryWarningVoltage = 101; // 3.7 each for 3S |
| 108,7 → 100,7 |
| staticParams.gyroPID[i].P = 40; |
| staticParams.gyroPID[i].I = 40; |
| staticParams.gyroPID[i].D = 40; |
| staticParams.gyroPID[i].iMax = 45; |
| staticParams.gyroPID[i].iMax = 60; |
| } |
| staticParams.stickIElevator = 40; |
| 158,7 → 150,7 |
| /***************************************************/ |
| void channelMap_default(void) { |
| channelMap.RCPolarity = 1; |
| channelMap.HWTrim = 110; |
| channelMap.HWTrim = 10; |
| channelMap.variableOffset = 131; |
| channelMap.channels[CH_ELEVATOR] = 1; |
| channelMap.channels[CH_AILERONS] = 0; |
| /branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.h |
|---|
| 66,10 → 66,6 |
| FLIGHT_MODE_ANGLES |
| } FlightMode_t; |
| #define CONTROL_SERVO_REVERSE_ELEVATOR 1 |
| #define CONTROL_SERVO_REVERSE_AILERONS 2 |
| #define CONTROL_SERVO_REVERSE_RUDDER 4 |
| typedef struct { |
| uint8_t SWMajor; |
| uint8_t SWMinor; |
| 92,8 → 88,6 |
| /*PMM*/uint8_t output0Timing; |
| /*PMM*/uint8_t output1Timing; |
| uint8_t gimbalServoManualControl[2]; |
| /* P */uint8_t userParams[8]; |
| } DynamicParams_t; |
| 104,13 → 98,7 |
| uint8_t min, max; |
| } MMXLATION; |
| /* |
| typedef struct { |
| uint8_t sourceIdx, targetIdx; |
| } XLATION; |
| */ |
| typedef struct { |
| uint8_t RCPolarity; // 1=positive, 0=negative. Use positive with Futaba receiver, negative with FrSky. |
| uint8_t HWTrim; |
| uint8_t variableOffset; |
| 130,11 → 118,9 |
| } sensorOffset_t; |
| typedef struct { |
| uint8_t manualControl; |
| uint8_t stabilizationFactor; |
| uint8_t minValue; |
| uint8_t maxValue; |
| uint8_t flags; |
| uint8_t reverse; |
| int8_t minValue; |
| int8_t maxValue; |
| } Servo_t; |
| #define SERVO_STABILIZATION_REVERSE 1 |
| 168,14 → 154,8 |
| // Servos |
| uint8_t servoCount; |
| uint8_t servosReverse; |
| Servo_t servos[3]; |
| uint8_t controlServoMinValue; |
| uint8_t controlServoMaxValue; |
| uint8_t gimbalServoMaxManualSpeed; |
| Servo_t gimbalServoConfigurations[2]; // [PITCH, ROLL] |
| // Outputs |
| output_flash_t outputFlash[2]; |
| uint8_t outputDebugMask; |
| /branches/dongfang_FC_fixedwing/arduino_atmega328/flight.c |
|---|
| 23,7 → 23,6 |
| * Error integrals. |
| */ |
| uint8_t reverse[3]; |
| int32_t maxError[3]; |
| int32_t IPart[3] = { 0, 0, 0 }; |
| PID_t airspeedPID[3]; |
| 45,10 → 44,6 |
| void flight_updateFlightParametersToFlightMode(void) { |
| debugOut.analog[16] = currentFlightMode; |
| reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
| reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
| reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
| // At a switch to angles, we want to kill errors first. |
| // This should be triggered only once per mode change! |
| if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| 102,13 → 97,13 |
| int32_t tmp; |
| tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
| if (reverse[PITCH]) target[PITCH] += tmp; else target[PITCH] -= tmp; |
| if (staticParams.servos[PITCH].reverse) target[PITCH] += tmp; else target[PITCH] -= tmp; |
| tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
| if (reverse[ROLL]) target[ROLL] += tmp; else target[ROLL] -= tmp; |
| if (staticParams.servos[ROLL].reverse) target[ROLL] += tmp; else target[ROLL] -= tmp; |
| tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
| if (reverse[YAW]) target[YAW] += tmp; else target[YAW] -= tmp; |
| if (staticParams.servos[YAW].reverse) target[YAW] += tmp; else target[YAW] -= tmp; |
| for (axis = PITCH; axis <= YAW; axis++) { |
| if (target[axis] > OVER180) { |
| 192,7 → 187,7 |
| } |
| // Add I parts here... these are integrated errors. |
| if (reverse[axis]) |
| if (staticParams.servos[axis].reverse) |
| term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis]; |
| else |
| term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
| /branches/dongfang_FC_fixedwing/arduino_atmega328/flight.h |
|---|
| 14,8 → 14,8 |
| #define YAW 2 |
| #define THROTTLE 3 |
| #define LOG_STICK_SCALE 8 |
| #define LOG_P_SCALE 9 |
| #define LOG_STICK_SCALE 6 |
| #define LOG_P_SCALE 8 |
| #define LOG_I_SCALE 12 |
| #define LOG_D_SCALE 8 |
| /branches/dongfang_FC_fixedwing/arduino_atmega328/main.c |
|---|
| 113,7 → 113,7 |
| beepBatteryAlarm(); |
| } |
| } |
| calculateFeaturedServoValues(); |
| //calculateFeaturedServoValues(); |
| timer = setDelay(20); // every 20 ms |
| } |
| /branches/dongfang_FC_fixedwing/arduino_atmega328/makefile |
|---|
| 1,6 → 1,6 |
| #-------------------------------------------------------------------- |
| # MCU name |
| MCU = atmega328 |
| MCU = atmega328p |
| F_CPU = 16000000 |
| QUARZ = 16MHZ |
| 142,7 → 142,7 |
| #AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep |
| #avrdude -c avrispv2 -P usb -p m32 -U flash:w:blink.hex |
| AVRDUDE_FLAGS = -p $(MCU) -c $(AVRDUDE_PROGRAMMER) -B 1 -F |
| AVRDUDE_FLAGS = -p $(MCU) -c $(AVRDUDE_PROGRAMMER) -B 20 -F |
| # Uncomment the following if you want avrdude's erase cycle counter. |
| # Note that this counter needs to be initialized first using -Yn, |
| /branches/dongfang_FC_fixedwing/arduino_atmega328/timer2.c |
|---|
| 10,26 → 10,24 |
| #ifdef COARSERESOLUTION |
| #define NEUTRAL_PULSELENGTH ((int16_t)(F_CPU/32000*1.5f + 0.5f)) |
| #define STABILIZATION_LOG_DIVIDER 6 |
| #define SERVOLIMIT ((int16_t)(F_CPU/32000*0.8f + 0.5f)) |
| #define SCALE_FACTOR 4 |
| #define SERVO_NORMAL_LIMIT ((int16_t)(F_CPU/32000*0.5f + 0.5f)) |
| #define SERVO_ABS_LIMIT ((int16_t)(F_CPU/32000*0.8f + 0.5f)) |
| //#define SCALE_FACTOR 4 |
| #define CS2 ((1<<CS21)|(1<<CS20)) |
| #else |
| #define NEUTRAL_PULSELENGTH ((int16_t)(F_CPU/8000.0f * 1.5f + 0.5f)) |
| #define STABILIZATION_LOG_DIVIDER 4 |
| #define SERVOLIMIT ((int16_t)(F_CPU/8000.0f * 0.8f + 0.5f)) |
| #define SCALE_FACTOR 16 |
| #define SERVO_NORMAL_LIMIT ((int16_t)(F_CPU/8000.0f * 0.5f + 0.5f)) |
| #define SERVO_ABS_LIMIT ((int16_t)(F_CPU/8000.0f * 0.8f + 0.5f)) |
| //#define SCALE_FACTOR 16 |
| #define CS2 (1<<CS21) |
| #endif |
| #define FRAMELENGTH ((uint16_t)(NEUTRAL_PULSELENGTH + SERVOLIMIT) * (uint16_t)staticParams.servoCount + 128) |
| #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
| #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
| #define FRAMELENGTH ((uint16_t)(NEUTRAL_PULSELENGTH + SERVO_ABS_LIMIT) * (uint16_t)staticParams.servoCount + 128) |
| volatile uint8_t recalculateServoTimes = 0; |
| volatile uint16_t servoValues[MAX_SERVOS]; |
| volatile uint16_t previousManualValues[2]; |
| //volatile uint8_t recalculateServoTimes = 0; |
| //volatile uint16_t previousManualValues[2]; |
| #define HEF4017R_ON PORTD |= (1<<PORTD3) |
| #define HEF4017R_OFF PORTD &= ~(1<<PORTD3) |
| 80,15 → 78,20 |
| TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
| TIMSK2 |= (1 << OCIE2A); |
| for (uint8_t axis=0; axis<2; axis++) |
| previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR; |
| //for (uint8_t axis=0; axis<2; axis++) |
| // previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR; |
| for (uint8_t i=0; i<MAX_SERVOS; i++) |
| servoValues[i] = NEUTRAL_PULSELENGTH; |
| SREG = sreg; |
| } |
| /***************************************************** |
| * Control (camera gimbal etc.) servos |
| * Makes no sense as long as we have no acc. reference. |
| *****************************************************/ |
| /* |
| int16_t calculateStabilizedServoAxis(uint8_t axis) { |
| int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about |
| // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
| 110,7 → 113,9 |
| previousManualValues[axis] = manualValue; |
| return manualValue; |
| } |
| */ |
| /* |
| // add stabilization and manual, apply soft position limits. |
| // All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
| int16_t featuredServoValue(uint8_t axis) { |
| 126,17 → 131,28 |
| // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
| return value + NEUTRAL_PULSELENGTH; |
| } |
| */ |
| void calculateControlServoValues(void) { |
| int16_t value; |
| for (uint8_t axis=0; axis<4; axis++) { |
| value = controlServos[axis]; |
| if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
| else if (value > SERVOLIMIT) value = SERVOLIMIT; |
| // Apply configurable limits. These are signed: +-128 is twice the normal +- 0.5 ms limit and +- 64 is normal. |
| int16_t min = (staticParams.servos[axis].minValue * SERVO_NORMAL_LIMIT) >> 6; |
| int16_t max = (staticParams.servos[axis].maxValue * SERVO_NORMAL_LIMIT) >> 6; |
| if (value < min) value = min; |
| else if (value > max) value = max; |
| if (value < -SERVO_ABS_LIMIT) value = -SERVO_ABS_LIMIT; |
| else if (value > SERVO_ABS_LIMIT) value = SERVO_ABS_LIMIT; |
| servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
| } |
| } |
| /* |
| void calculateFeaturedServoValues(void) { |
| int16_t value; |
| uint8_t axis; |
| 155,6 → 171,7 |
| recalculateServoTimes = 0; |
| } |
| */ |
| ISR(TIMER2_COMPA_vect) { |
| static uint16_t remainingPulseTime; |
| 171,7 → 188,7 |
| // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
| remainingPulseTime = FRAMELENGTH - sumOfPulseTimes; |
| sumOfPulseTimes = servoIndex = 0; |
| recalculateServoTimes = 1; |
| //recalculateServoTimes = 1; |
| HEF4017R_ON; |
| } |
| } |