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