/branches/dongfang_FC_fixedwing/arduino_atmega328/analog.c |
---|
41,7 → 41,7 |
/* |
* Airspeed |
*/ |
uint32_t airpressure; |
int16_t airpressure; |
uint16_t airspeedVelocity = 0; |
//int16_t airpressureWindow[AIRPRESSURE_WINDOW_LENGTH]; |
//uint8_t airpressureWindowIdx = 0; |
211,6 → 211,7 |
} |
void startAnalogConversionCycle(void) { |
twimaster_startCycle(); |
// Stop the sampling. Cycle is over. |
for (uint8_t i = 0; i<8; i++) { |
ADSensorInputs[i] = 0; |
220,7 → 221,6 |
adChannel = AD_AIRPRESSURE; |
ADMUX = (ADMUX & 0xE0) | adChannel; |
startADC(); |
twimaster_startCycle(); |
sensorDataReady = 0; |
} |
301,7 → 301,7 |
} |
// probably wanna aim at 1/10 m/s/unit. |
#define LOG_AIRSPEED_FACTOR 2 |
#define LOG_AIRSPEED_FACTOR 0 |
void analog_updateAirspeed(void) { |
uint16_t rawAirpressure = ADSensorInputs[AD_AIRPRESSURE]; |
369,7 → 369,7 |
} |
void analog_calibrate(void) { |
#define OFFSET_CYCLES 32 |
#define OFFSET_CYCLES 120 |
uint8_t i, axis; |
int32_t offsets[4] = { 0, 0, 0, 0}; |
/branches/dongfang_FC_fixedwing/arduino_atmega328/commands.c |
---|
7,28 → 7,24 |
#include "output.h" |
#include "rc.h" |
#ifdef USE_MK3MAG |
// TODO: Kick that all outa here! |
uint8_t compassCalState = 0; |
#endif |
void commands_handleCommands(void) { |
/* |
* Get the current command (start/stop motors, calibrate), if any. |
*/ |
uint8_t command = controlMixer_getCommand(); |
uint8_t repeated = controlMixer_isCommandRepeated(); |
uint8_t argument = controlMixer_getArgument(); |
if (!isFlying) { |
if (command == COMMAND_GYROCAL && !repeated) { |
// Gyro calinbration, with or without selecting a new parameter-set. |
paramSet_readFromEEProm(1); |
if (command == COMMAND_RCCAL && !repeated) { |
RC_calibrate(); |
rcTrim_writeToEEProm(); |
beepNumber(2); // 2 beeps means RC. |
} else if (command == COMMAND_GYROCAL && !repeated) { |
// Gyro calibration, with or without selecting a new parameter-set. |
// paramSet_readFromEEProm(1); |
analog_calibrate(); |
attitude_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(1); |
beepNumber(1); // 1 beep means gyro. |
} else if (command == COMMAND_CHMOD && !repeated) { |
configuration_setFlightParameters(argument); |
} |
} // end !MOTOR_RUN condition. |
} |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/commands.h |
---|
5,7 → 5,8 |
#define COMMAND_NONE 0 |
#define COMMAND_GYROCAL 1 |
#define COMMAND_CHMOD 2 |
#define COMMAND_RCCAL 2 |
#define COMMAND_CHMOD 3 |
void commands_handleCommands(void); |
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.c |
---|
9,6 → 9,7 |
int16_t variables[VARIABLE_COUNT]; |
ParamSet_t staticParams; |
ChannelMap_t channelMap; |
RCTrim_t rcTrim; |
IMUConfig_t IMUConfig; |
volatile DynamicParams_t dynamicParams; |
33,8 → 34,8 |
{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, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255}, |
{offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),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}, |
}; |
89,19 → 90,19 |
// Servos |
staticParams.servoCount = 7; |
staticParams.servosReverse = CONTROL_SERVO_REVERSE_ELEVATOR | CONTROL_SERVO_REVERSE_RUDDER; |
staticParams.gimbalServoMaxManualSpeed = 10; |
for (uint8_t i=0; i<2; i++) { |
staticParams.servoConfigurations[i].manualControl = 128; |
staticParams.servoConfigurations[i].stabilizationFactor = 0; |
staticParams.servoConfigurations[i].minValue = 32; |
staticParams.servoConfigurations[i].maxValue = 224; |
staticParams.servoConfigurations[i].flags = 0; |
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 |
staticParams.emergencyThrottle = 35; |
staticParams.emergencyFlightDuration = 30; |
for (uint8_t i=0; i<3; i++) { |
staticParams.gyroPID[i].P = 80; |
110,9 → 111,9 |
staticParams.gyroPID[i].iMax = 45; |
} |
staticParams.stickIElevator = 80; |
staticParams.stickIAilerons = 120; |
staticParams.stickIRudder = 40; |
staticParams.stickIElevator = 60; |
staticParams.stickIAilerons = 80; |
staticParams.stickIRudder = 25; |
// Outputs |
staticParams.outputFlash[0].bitmask = 1; //0b01011111; |
147,6 → 148,9 |
IMUConfig.gyroDWindowLength = 3; |
IMUConfig.gyroQuadrant = 0; |
IMUConfig.imuReversedFlags = 0; |
IMUConfig.gyroCalibrationTweak[0] = |
IMUConfig.gyroCalibrationTweak[1] = |
IMUConfig.gyroCalibrationTweak[2] = 0; |
} |
/***************************************************/ |
153,7 → 157,9 |
/* Default Values for R/C Channels */ |
/***************************************************/ |
void channelMap_default(void) { |
channelMap.HWTrim = 0; |
channelMap.RCPolarity = 1; |
channelMap.HWTrim = 192; |
channelMap.variableOffset = 128; |
channelMap.channels[CH_ELEVATOR] = 1; |
channelMap.channels[CH_AILERONS] = 0; |
channelMap.channels[CH_THROTTLE] = 2; |
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.h |
---|
42,6 → 42,7 |
uint8_t gyroActivityDamping; |
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung) |
uint8_t driftCompLimit; // limit for gyrodrift compensation |
int8_t gyroCalibrationTweak[3]; |
} IMUConfig_t; |
extern IMUConfig_t IMUConfig; |
92,7 → 93,7 |
/*PMM*/uint8_t output0Timing; |
/*PMM*/uint8_t output1Timing; |
uint8_t servoManualControl[2]; |
uint8_t gimbalServoManualControl[2]; |
/* P */uint8_t userParams[8]; |
} DynamicParams_t; |
111,13 → 112,21 |
*/ |
typedef struct { |
uint8_t HWTrim; |
uint8_t variableOffset; |
uint8_t RCPolarity; // 1=positive, 0=negative. Use positive with Futaba receiver, negative with FrSky. |
uint8_t HWTrim; |
uint8_t variableOffset; |
uint8_t channels[MAX_CHANNELS]; |
} ChannelMap_t; |
extern ChannelMap_t channelMap; |
// With fixed wing, we need some way to trim the plane. This is done during a trim flight without gyros activated, |
// and then save in a succeeding gyro calibration command. |
typedef struct { |
int16_t trim[MAX_CHANNELS]; |
} RCTrim_t; |
extern RCTrim_t rcTrim; |
typedef struct { |
int16_t offsets[3]; |
} sensorOffset_t; |
/branches/dongfang_FC_fixedwing/attitude.c |
---|
80,10 → 80,7 |
* The rate variable end up in a range of about [-1024, 1023]. |
*************************************************************************/ |
void getAnalogData(void) { |
uint8_t axis; |
analog_update(); |
for (axis = PITCH; axis <= YAW; axis++) { |
} |
} |
void integrate(void) { |
/branches/dongfang_FC_fixedwing/commands.c |
---|
17,7 → 17,7 |
rcTrim_writeToEEProm(); |
beepNumber(2); // 2 beeps means RC. |
} else if (command == COMMAND_GYROCAL && !repeated) { |
// Gyro calinbration, with or without selecting a new parameter-set. |
// Gyro calibration, with or without selecting a new parameter-set. |
// paramSet_readFromEEProm(1); |
analog_calibrate(); |
attitude_setNeutral(); |
/branches/dongfang_FC_fixedwing/configuration.c |
---|
21,6 → 21,7 |
VersionInfo_t versionInfo; |
FlightMode_t currentFlightMode = FLIGHT_MODE_MANUAL; |
// updated by considering airspeed.. |
volatile uint16_t isFlying= 0; |
const MMXLATION XLATIONS[] = { |
154,7 → 155,7 |
staticParams.batteryWarningVoltage = 101; // 3.7 each for 3S |
for (uint8_t i=0; i<3; i++) { |
staticParams.gyroPID[i].P = 200; |
staticParams.gyroPID[i].P = 80; |
staticParams.gyroPID[i].I = 40; |
staticParams.gyroPID[i].D = 40; |
staticParams.gyroPID[i].iMax = 45; |
207,10 → 208,7 |
/* Default Values for R/C Channels */ |
/***************************************************/ |
void channelMap_default(void) { |
channelMap.RCPolarity = 1; |
channelMap.HWTrim = 192; |
channelMap.variableOffset = 128; |
channelMap.channels[CH_ELEVATOR] = 1; |
channelMap.channels[CH_ELEVATOR] = 1; |
channelMap.channels[CH_AILERONS] = 0; |
channelMap.channels[CH_THROTTLE] = 2; |
channelMap.channels[CH_RUDDER] = 3; |
/branches/dongfang_FC_fixedwing/configuration.h |
---|
157,9 → 157,10 |
uint8_t stickIAilerons; |
uint8_t stickIRudder; |
uint8_t externalControl; // for serial Control |
uint8_t IFactor; |
uint8_t externalControl; // for serial Control |
uint8_t batteryWarningVoltage; |
// Airspeed |
/branches/dongfang_FC_fixedwing/flight.c |
---|
130,19 → 130,37 |
target[axis] += OVER360; |
} |
//if (reverse[axis]) |
/* This is the difference limitation only way. The 2 subtrahends stay unmodified. */ |
error[axis] = attitude[axis] - target[axis]; |
// else |
// error[axis] = attitude[axis] - target[axis]; |
if (error[axis] > OVER180) { |
error[axis] -= OVER360; |
} else if (error[axis] <= -OVER180) { |
error[axis] += OVER360; |
} |
// Believe it or not, the below limiter does NOT solve the wrapping problem. Must do explicitly. |
if (error[axis] > maxError[axis]) { |
error[axis] = maxError[axis]; |
} else if (error[axis] < -maxError[axis]) { |
error[axis] =- maxError[axis]; |
error[axis] = -maxError[axis]; |
} else { |
// update I parts here for angles mode. Ĩ parts in rate mode is something different. |
// update I parts here for angles mode. I parts in rate mode is something different. |
} |
/* |
* This is the beginning of a version that adjusts the target to differ from the attitude |
* by a limited amount. This will eliminate memory over large errors but also knock target angles. |
* Idea: The limit could be calculated from the max. servo deflection divided by I factor... |
* |
*/ |
/* |
if(abs(attitude[axis]-target[axis]) > OVER180) { |
if(target[axis] > attitude[axis]) { |
} |
} |
*/ |
/************************************************************************/ |
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |