/branches/dongfang_FC_fixedwing/.cproject |
---|
3,8 → 3,8 |
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage"> |
<storageModule moduleId="org.eclipse.cdt.core.settings"> |
<cconfiguration id="0.142097617"> |
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.142097617" moduleId="org.eclipse.cdt.core.settings" name="Default"> |
<cconfiguration id="0.40610304"> |
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.40610304" moduleId="org.eclipse.cdt.core.settings" name="Default"> |
<externalSettings/> |
<extensions> |
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> |
16,38 → 16,195 |
</extensions> |
</storageModule> |
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> |
<configuration buildProperties="" description="" id="0.142097617" name="Default" parent="org.eclipse.cdt.build.core.prefbase.cfg"> |
<folderInfo id="0.142097617." name="/" resourcePath=""> |
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.648869171" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain"> |
<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.648869171.1366251437" name=""/> |
<builder id="org.eclipse.cdt.build.core.settings.default.builder.1198454805" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1748574311" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.580525788" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.209091053" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
<configuration buildProperties="" description="" id="0.40610304" name="Default" parent="org.eclipse.cdt.build.core.prefbase.cfg"> |
<folderInfo id="0.40610304." name="/" resourcePath=""> |
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.1321557107" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain"> |
<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.1321557107.1645212802" name=""/> |
<builder id="org.eclipse.cdt.build.core.settings.default.builder.1993014636" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1525661393" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.1675765699" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1137211286" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
</tool> |
<tool id="org.eclipse.cdt.build.core.settings.holder.297906706" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.2065654697" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.1502448144" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.2046397049" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
</tool> |
<tool id="org.eclipse.cdt.build.core.settings.holder.187554179" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1912300390" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.327729409" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.585283599" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
</tool> |
</toolChain> |
</folderInfo> |
</configuration> |
</storageModule> |
<storageModule moduleId="scannerConfiguration"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="makefileGenerator"> |
<runAction arguments="-E -P -v -dD" command="" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/${specs_file}"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'g++ -E -P -v -dD "${plugin_state_location}/specs.cpp"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/specs.c"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<scannerConfigBuildInfo instanceId="0.40610304"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="makefileGenerator"> |
<runAction arguments="-E -P -v -dD" command="" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/${specs_file}"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'g++ -E -P -v -dD "${plugin_state_location}/specs.cpp"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/specs.c"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
</storageModule> |
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/> |
</cconfiguration> |
</storageModule> |
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> |
<project id="dongfang_FC_fixedwing.forward.null.1616337749" name="dongfang_FC_fixedwing.forward"/> |
<project id="dongfang_FC_fixedwing.null.1278448474" name="dongfang_FC_fixedwing"/> |
</storageModule> |
<storageModule moduleId="scannerConfiguration"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
<scannerConfigBuildInfo instanceId="0.142097617"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
</scannerConfigBuildInfo> |
<scannerConfigBuildInfo instanceId="0.1994915586"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
</scannerConfigBuildInfo> |
</storageModule> |
</cproject> |
/branches/dongfang_FC_fixedwing/analog.c |
---|
7,6 → 7,7 |
#include "attitude.h" |
#include "sensors.h" |
#include "printf_P.h" |
#include "isqrt.h" |
// for Delay functions |
#include "timer0.h" |
44,8 → 45,8 |
int16_t gyroD[3]; |
int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH]; |
uint8_t gyroDWindowIdx = 0; |
int16_t dHeight; |
uint32_t gyroActivity; |
//int16_t dHeight; |
//uint32_t gyroActivity; |
/* |
* Offset values. These are the raw gyro and acc. meter sums when the copter is |
116,6 → 117,7 |
* Airspeed |
*/ |
uint16_t simpleAirPressure; |
uint16_t airspeedVelocity; |
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered. |
// int32_t filteredAirPressure; |
375,12 → 377,6 |
gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
/* |
measureGyroActivity(gyroD[PITCH]); |
measureGyroActivity(gyroD[ROLL]); |
measureGyroActivity(yawGyro); |
*/ |
if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) { |
gyroDWindowIdx = 0; |
} |
389,6 → 385,7 |
void analog_updateAirPressure(void) { |
uint16_t rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
simpleAirPressure = rawAirPressure; |
airspeedVelocity = isqrt16(simpleAirPressure); |
} |
void analog_updateBatteryVoltage(void) { |
417,14 → 414,6 |
gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING; |
} |
/* |
if (accOffset_readFromEEProm()) { |
printf("acc. meter offsets invalid%s",recal); |
accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY; |
accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z; |
} |
*/ |
// Noise is relative to offset. So, reset noise measurements when changing offsets. |
for (uint8_t i=PITCH; i<=YAW; i++) { |
gyroNoisePeak[i] = 0; |
437,7 → 426,7 |
// Therefore run measurement for 100ms to achive stable readings |
delay_ms_with_adc_measurement(100, 0); |
gyroActivity = 0; |
// gyroActivity = 0; |
} |
void analog_calibrateGyros(void) { |
/branches/dongfang_FC_fixedwing/analog.h |
---|
116,7 → 116,7 |
extern sensorOffset_t gyroOffset; |
//extern sensorOffset_t accOffset; |
extern sensorOffset_t gyroAmplifierOffset; |
//extern sensorOffset_t gyroAmplifierOffset; |
/* |
* This is not really for external use - but the ENC-03 gyro modules needs it. |
136,7 → 136,7 |
* its motors running. |
*/ |
extern uint16_t gyroNoisePeak[3]; |
extern uint16_t accNoisePeak[3]; |
//extern uint16_t accNoisePeak[3]; |
/* |
* Air pressure. |
168,31 → 168,9 |
This is OCR2 = 143.15 at 1.5V in --> simple pressure = |
*/ |
#define AIRPRESSURE_OVERSAMPLING 14 |
#define AIRPRESSURE_FILTER 8 |
// Minimum A/D value before a range change is performed. |
#define MIN_RAWPRESSURE (200 * 2) |
// Maximum A/D value before a range change is performed. |
#define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE) |
#define MIN_RANGES_EXTRAPOLATION 15 |
#define MAX_RANGES_EXTRAPOLATION 240 |
#define PRESSURE_EXTRAPOLATION_COEFF 25L |
#define AUTORANGE_WAIT_FACTOR 1 |
#define ABS_ALTITUDE_OFFSET 108205 |
extern uint16_t simpleAirPressure; |
/* |
* At saturation, the filteredAirPressure may actually be (simulated) negative. |
*/ |
extern int32_t filteredAirPressure; |
extern uint16_t airspeedVelocity; |
extern int16_t magneticHeading; |
extern uint32_t gyroActivity; |
/* |
* Flag: Interrupt handler has done all A/D conversion and processing. |
*/ |
232,10 → 210,4 |
*/ |
//void analog_calibrateAcc(void); |
void analog_setGround(void); |
int32_t analog_getHeight(void); |
int16_t analog_getDHeight(void); |
#endif //_ANALOG_H |
/branches/dongfang_FC_fixedwing/commands.c |
---|
20,7 → 20,8 |
uint8_t repeated = controlMixer_isCommandRepeated(); |
uint8_t argument = controlMixer_getArgument(); |
if (!(MKFlags & MKFLAG_MOTOR_RUN)) { |
// TODO! Mode change gadget of some kind. |
if (!isMotorRunning) { |
if (command == COMMAND_GYROCAL && !repeated) { |
// Gyro calinbration, with or without selecting a new parameter-set. |
paramSet_readFromEEProm(1); |
28,19 → 29,8 |
attitude_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(1); |
} else if (command == COMMAND_CHMOD && !repeated) { |
configuration_setFlightParameters(argument); |
} |
// save the ACC neutral setting to eeprom |
/* |
else { |
if (command == COMMAND_ACCCAL && !repeated) { |
// Run gyro and acc. meter calibration but do not repeat it. |
analog_calibrateAcc(); |
attitude_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(getActiveParamSet()); |
} |
} |
*/ |
} // end !MOTOR_RUN condition. |
} |
/branches/dongfang_FC_fixedwing/commands.h |
---|
8,6 → 8,7 |
*/ |
#define COMMAND_NONE 0 |
#define COMMAND_GYROCAL 1 |
#define COMMAND_CHMOD 2 |
void commands_handleCommands(void); |
/branches/dongfang_FC_fixedwing/configuration.c |
---|
20,8 → 20,8 |
VersionInfo_t versionInfo; |
// MK flags. TODO: Replace by enum. State machine. |
uint16_t isFlying = 0; |
volatile uint8_t MKFlags = 0; |
FlightMode_t currentFlightMode = FLIGHT_MODE_MANUAL; |
volatile uint8_t isMotorRunning = 0; |
const MMXLATION XLATIONS[] = { |
{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255}, |
103,20 → 103,19 |
GRN_OFF; |
} |
void configuration_setFlightParameters() { |
// Implement: Update of stuff in flight.c |
void configuration_setFlightParameters(uint8_t newFlightMode) { |
updateFlightParametersToFlightMode(currentFlightMode = newFlightMode); |
} |
// Called after a change in configuration parameters, as a hook for modules to take over changes. |
void configuration_paramSetDidChange(void) { |
// This should be OK to do here as long as we don't change parameters during emergency flight. We don't. |
configuration_setFlightParameters(); |
configuration_setFlightParameters(currentFlightMode); |
// Immediately load changes to output, and also signal the paramset change. |
output_init(); |
} |
void setOtherDefaults(void) { |
// Control |
staticParams.externalControl = 0; |
staticParams.IFactor = 52; |
/branches/dongfang_FC_fixedwing/configuration.h |
---|
26,6 → 26,17 |
#define FC_ERROR1_RES2 0x40 |
#define FC_ERROR1_RES3 0x80 |
typedef enum { |
FLIGHT_MODE_NONE, |
FLIGHT_MODE_MANUAL, |
FLIGHT_MODE_RATE, |
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; |
39,18 → 50,12 |
typedef struct { |
/*PMM*/uint8_t gyroPitchD; |
/* P */uint8_t gyroRollD; |
/* P */uint8_t gyroRollD;\ |
/* P */uint8_t gyroYawD; |
// Control |
/* P */uint8_t externalControl; |
// Height control |
/*PMM*/uint8_t heightP; |
/* P */uint8_t heightI; |
/*PMM*/uint8_t heightD; |
/* P */uint8_t heightSetting; |
// Output and servo |
/*PMM*/uint8_t output0Timing; |
/*PMM*/uint8_t output1Timing; |
123,6 → 128,12 |
extern IMUConfig_t IMUConfig; |
typedef struct { |
uint8_t P; |
uint8_t I; |
uint8_t D; |
} PID_t; |
// values above 250 representing poti1 to poti4 |
typedef struct { |
// Global bitflags |
135,18 → 146,8 |
uint8_t levelCorrection[2]; |
// Control |
uint8_t gyroPitchP; |
uint8_t gyroPitchI; |
uint8_t gyroPitchD; |
PID_t gyroPID[3]; |
uint8_t gyroRollP; |
uint8_t gyroRollI; |
uint8_t gyroRollD; |
uint8_t gyroYawP; |
uint8_t gyroYawI; |
uint8_t gyroYawD; |
uint8_t stickIElevator; |
uint8_t stickIAilerons; |
uint8_t stickIRudder; |
160,6 → 161,8 |
uint8_t emergencyFlightDuration; |
// Servos |
uint8_t controlServosReverse; |
uint8_t servoCount; |
uint8_t servoManualMaxSpeed; |
servo_t servoConfigurations[2]; // [PITCH, ROLL] |
178,16 → 181,6 |
extern ParamSet_t staticParams; |
// MKFlags |
#define MKFLAG_MOTOR_RUN (1<<0) |
//#define MKFLAG_FLY (1<<1) |
#define MKFLAG_CALIBRATE (1<<2) |
#define MKFLAG_START (1<<3) |
#define MKFLAG_EMERGENCY_FLIGHT (1<<4) |
#define MKFLAG_LOWBAT (1<<5) |
#define MKFLAG_RESERVE2 (1<<6) |
#define MKFLAG_RESERVE3 (1<<7) |
// bit mask for staticParams.bitConfig |
#define CFG_SIMPLE_HEIGHT_CONTROL (1<<0) |
#define CFG_SIMPLE_HC_HOLD_SWITCH (1<<1) |
223,22 → 216,18 |
#define VARIABLE_COUNT 8 |
extern volatile uint8_t MKFlags; |
extern uint8_t requiredMotors; |
extern int16_t variables[VARIABLE_COUNT]; // The "Poti"s. |
extern uint8_t boardRelease; |
extern uint8_t CPUType; |
extern volatile uint8_t MKFlags; |
extern uint16_t isFlying; |
extern volatile uint8_t isMotorRunning; |
extern FlightMode_t currentFlightMode; |
void IMUConfig_default(void); |
void channelMap_default(void); |
void paramSet_default(uint8_t setnumber); |
void mixerMatrix_default(void); |
void configuration_setNormalFlightParameters(void); |
void configuration_setFailsafeFlightParameters(void); |
void configuration_setFlightParameters(uint8_t newFlightMode); |
void configuration_applyVariablesToParams(void); |
void setCPUType(void); |
/branches/dongfang_FC_fixedwing/controlMixer.c |
---|
43,9 → 43,6 |
} |
EC_setNeutral(); |
FC_setNeutral(); // FC is FailsafeControl, not FlightCtrl. |
// This is to set the home pos in navi. |
// MKFlags |= MKFLAG_CALIBRATE; |
} |
/* |
113,11 → 110,9 |
int16_t tempPRTY[4] = { 0, 0, 0, 0 }; |
// Decode commands. |
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
: COMMAND_NONE; |
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE; |
uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() |
: COMMAND_NONE; |
uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE; |
// Update variables ("potis"). |
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
/branches/dongfang_FC_fixedwing/controlMixer.h |
---|
96,16 → 96,6 |
extern uint8_t controlMixer_didReceiveSignal; |
/* |
* The controls operate in [-1024, 1024] just about. |
* Throttle is [0..255] just about. |
*/ |
// Scale controls to 1 byte: |
#define CONTROL_SCALING (1024/256) |
// Scale throttle levels to byte: |
#define MOTOR_SCALING (1024/256) |
/* |
* Gets the argument for the current command (a number). |
* |
* Stick position to argument values (for stick control): |
/branches/dongfang_FC_fixedwing/eeprom.c |
---|
162,14 → 162,6 |
/***************************************************/ |
/* Sensor offsets */ |
/***************************************************/ |
uint8_t gyroAmplifierOffset_readFromEEProm(void) { |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroAmplifierOffset, EEPROM_ADR_GYROAMPLIFIER, sizeof(sensorOffset_t)); |
} |
void gyroAmplifierOffset_writeToEEProm(void) { |
return writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroAmplifierOffset, EEPROM_ADR_GYROAMPLIFIER, sizeof(sensorOffset_t)); |
} |
uint8_t gyroOffset_readFromEEProm(void) { |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t)); |
} |
177,13 → 169,3 |
void gyroOffset_writeToEEProm(void) { |
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t)); |
} |
/* |
uint8_t accOffset_readFromEEProm(void) { |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t)); |
} |
void accOffset_writeToEEProm(void) { |
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t)); |
} |
*/ |
/branches/dongfang_FC_fixedwing/failsafeControl.c |
---|
4,52 → 4,13 |
#include "flight.h" |
#include "output.h" |
uint16_t emergencyFlightTime; |
void FC_periodicTaskAndPRTY(uint16_t* PRTY) { |
debugOut.analog[25] = emergencyFlightTime; |
// debugOut.analog[30] = controlMixer_getSignalQuality(); |
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
if (controlMixer_didReceiveSignal) |
if (controlMixer_didReceiveSignal) { |
beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
// There are the possibilities: We are not yet in emergency flight, we are already in emergency flight. |
if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) { |
if (isFlying > 256) { |
MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing |
// configuration_setFailsafeFlightParameters(); |
// Set the time in whole seconds. |
if (staticParams.emergencyFlightDuration |
> (65535 - (uint16_t)F_MAINLOOP) / (uint16_t)F_MAINLOOP) |
emergencyFlightTime = 0xffff; |
else |
emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration |
* (uint16_t)F_MAINLOOP; |
} |
} else { |
if (emergencyFlightTime) { |
emergencyFlightTime--; |
} else { |
// stop motors but stay in emergency flight. |
MKFlags &= ~(MKFLAG_MOTOR_RUN); |
} |
} |
// In either case, use e. throttle and neutral controls. TODO: If there is supposed to be a navi come-home, this should affect RC control only and not navi. |
PRTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle; // Set emergency throttle |
PRTY[CONTROL_ELEVATOR] = PRTY[CONTROL_AILERONS] = PRTY[CONTROL_RUDDER] = 0; |
} else { |
// Signal is OK. |
if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing |
// configuration_setNormalFlightParameters(); |
} |
} |
} |
void FC_setNeutral() { |
} |
/branches/dongfang_FC_fixedwing/flight.c |
---|
7,9 → 7,6 |
// Necessary for external control and motor test |
#include "uart0.h" |
// for scope debugging |
// #include "rc.h" |
#include "timer2.h" |
#include "attitude.h" |
#include "controlMixer.h" |
32,7 → 29,7 |
uint8_t reverse[3]; |
int32_t IPart[3] = { 0, 0, 0 }; |
int16_t servos[MAX_SERVOS]; |
int16_t controlServos[MAX_CONTROL_SERVOS]; |
/************************************************************************/ |
/* Neutral Readings */ |
46,36 → 43,33 |
target[YAW] = attitude[YAW]; |
} |
void switchToFlightMode(FlightMode_t flightMode) { |
// this should be followed by a call to switchToFlightMode!! |
void updateFlightParametersToFlightMode(uint8_t flightMode) { |
debugOut.analog[15] = flightMode; |
reverse[CONTROL_ELEVATOR] = staticParams.controlServosReverse |
& CONTROL_SERVO_REVERSE_ELEVATOR; |
reverse[CONTROL_AILERONS] = staticParams.controlServosReverse |
& CONTROL_SERVO_REVERSE_AILERONS; |
reverse[CONTROL_RUDDER] = staticParams.controlServosReverse |
& CONTROL_SERVO_REVERSE_RUDDER; |
for (uint8_t i = 0; i < 3; i++) { |
if (flightMode == FLIGHT_MODE_MANUAL) { |
pFactor[PITCH] = 0; |
pFactor[ROLL] = 0; |
pFactor[YAW] = 0; |
} else if (flightMode == FLIGHT_MODE_RATE) { |
pFactor[PITCH] = 0; //staticParams...; |
pFactor[ROLL] = 0; //staticParams...; |
pFactor[YAW] = 0; //staticParams...; |
pFactor[i] = 0; |
dFactor[i] = 0; |
} else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_ANGLES) { |
pFactor[i] = staticParams.gyroPID[i].P; |
dFactor[i] = staticParams.gyroPID[i].D; |
} |
if (flightMode == FLIGHT_MODE_MANUAL || FLIGHT_MODE_RATE) { |
iFactor[PITCH] = 0; |
iFactor[ROLL] = 0; |
iFactor[YAW] = 0; |
} else if (flightMode == FLIGHT_MODE_ANGLES) { |
iFactor[PITCH] = 0; //staticParams...; |
iFactor[ROLL] = 0; //staticParams...; |
iFactor[YAW] = 0; //staticParams...; |
// When entering this mode, we want to avoid jerks from accumulated uncorrected errors. |
target[PITCH] = attitude[PITCH]; |
target[ROLL] = attitude[ROLL]; |
target[YAW] = attitude[YAW]; |
} |
dFactor[PITCH] = staticParams.gyroPitchD / CONTROL_CONFIG_SCALE; |
dFactor[ROLL] = staticParams.gyroRollD / CONTROL_CONFIG_SCALE; |
dFactor[YAW] = staticParams.gyroYawD / CONTROL_CONFIG_SCALE; |
// TODO: Set reverse also. |
if (flightMode == FLIGHT_MODE_ANGLES) { |
iFactor[i] = staticParams.gyroPID[i].I; |
} else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) { |
iFactor[i] = 0; |
} |
} |
} |
/************************************************************************/ |
/* Main Flight Control */ |
148,10 → 142,10 |
debugOut.analog[14] = throttleTerm; |
debugOut.analog[15] = term[YAW]; |
for (uint8_t i = 0; i < MAX_SERVOS; i++) { |
for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) { |
int16_t tmp; |
if (servoTestActive) { |
servos[i] = ((int16_t) servoTest[i] - 128) * 4; |
controlServos[i] = ((int16_t) servoTest[i] - 128) * 4; |
} else { |
// Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
switch (i) { |
171,27 → 165,37 |
tmp = 0; |
} |
// These are all signed and in the same units as the RC stuff in rc.c. |
servos[i] = tmp; |
controlServos[i] = tmp; |
} |
} |
calculateControlServoValues; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugging |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if (!(--debugDataTimer)) { |
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[2] = attitude[YAW] / (GYRO_DEG_FACTOR / 10); |
debugOut.analog[0] = rate_PID[PITCH]; // in 0.1 deg |
debugOut.analog[1] = rate_PID[ROLL]; // in 0.1 deg |
debugOut.analog[2] = rate_PID[YAW]; |
debugOut.analog[3] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[4] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[5] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10); |
debugOut.analog[6] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[7] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[8] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
debugOut.analog[12] = term[PITCH]; |
debugOut.analog[13] = term[ROLL]; |
debugOut.analog[14] = term[YAW]; |
//DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
//DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
//debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg |
/branches/dongfang_FC_fixedwing/flight.h |
---|
13,16 → 13,10 |
#define ROLL 1 |
#define YAW 2 |
#define MAX_SERVOS 8 |
#define MAX_CONTROL_SERVOS 4 |
extern int16_t servos[MAX_SERVOS]; |
extern int16_t controlServos[MAX_CONTROL_SERVOS]; |
typedef enum { |
FLIGHT_MODE_MANUAL, |
FLIGHT_MODE_RATE, |
FLIGHT_MODE_ANGLES |
} FlightMode_t; |
void flight_control(void); |
void transmitMotorThrottleData(void); |
void flight_setNeutral(void); |
/branches/dongfang_FC_fixedwing/main.c |
---|
137,7 → 137,7 |
J4LOW; |
// Allow Serial Data Transmit if motors must not updated or motors are not running |
if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
if (!runFlightControl || !isMotorRunning) { |
usart0_transmitTxData(); |
} |
169,6 → 169,7 |
SPI_TransmitByte(); |
} |
#endif |
calculateFeaturedServoValues(); |
if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
/branches/dongfang_FC_fixedwing/makefile |
---|
66,7 → 66,7 |
# Even though the DOS/Win* filesystem matches both .s and .S the same, |
# it will preserve the spelling of the filenames, and gcc itself does |
# care about how the name is spelled on its command-line. |
ASRC = |
ASRC = isqrt.S |
# List any extra directories to look for include files here. |
# Each directory must be seperated by a space. |
/branches/dongfang_FC_fixedwing/output.c |
---|
86,7 → 86,7 |
*/ |
void beepNumber(uint8_t numbeeps) { |
while(numbeeps--) { |
if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren! |
if(isMotorRunning) return; //auf keinen Fall bei laufenden Motoren! |
beep(100); // 0.1 second |
delay_ms(250); // blocks 250 ms as pause to next beep, |
// this will block the flight control loop, |
/branches/dongfang_FC_fixedwing/rc.c |
---|
11,9 → 11,12 |
// The channel array is 0-based! |
volatile int16_t PPM_in[MAX_CHANNELS]; |
volatile uint8_t RCQuality; |
uint8_t lastRCCommand = COMMAND_NONE; |
uint8_t commandTimer = 0; |
uint8_t lastFlightMode = FLIGHT_MODE_NONE; |
/*************************************************************** |
* 16bit timer 1 is used to decode the PPM-Signal |
***************************************************************/ |
149,6 → 152,45 |
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
#define RC_SCALING 4 |
uint8_t getControlModeSwitch() { |
int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET; |
uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL : |
(channel > 256*2/3 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE); |
return flightMode; |
} |
// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice. |
// Maybe simply: Very very low throttle. |
// Throttle xlow for COMMAND_TIMER: GYROCAL (once). |
// mode switched: CHMOD |
uint8_t RC_getCommand(void) { |
uint8_t flightMode = getControlModeSwitch(); |
if (lastFlightMode != flightMode) { |
lastFlightMode = flightMode; |
lastRCCommand = COMMAND_CHMOD; |
return lastRCCommand; |
} |
int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET; |
if (channel <= -32) { // <= 900 us |
if (commandTimer == COMMAND_TIMER) { |
lastRCCommand = COMMAND_GYROCAL; |
} |
if (commandTimer <= COMMAND_TIMER) { |
commandTimer++; |
} |
} else commandTimer = 0; |
return lastRCCommand; |
} |
uint8_t RC_getArgument(void) { |
return lastFlightMode; |
} |
/* |
* Get Pitch, Roll, Throttle, Yaw values |
*/ |
155,11 → 197,14 |
void RC_periodicTaskAndPRTY(int16_t* PRTY) { |
if (RCQuality) { |
RCQuality--; |
PRTY[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR); |
PRTY[CONTROL_AILERONS] = RCChannel(CH_AILERONS); |
PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); |
PRTY[CONTROL_RUDDER] = RCChannel(CH_RUDDER); |
PRTY[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) * RC_SCALING; |
PRTY[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * RC_SCALING; |
PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * RC_SCALING; |
PRTY[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * RC_SCALING; |
debugOut.analog[16] = PRTY[CONTROL_ELEVATOR]; |
debugOut.analog[17] = PRTY[CONTROL_THROTTLE]; |
uint8_t command = COMMAND_NONE; //RC_getStickCommand(); |
if (lastRCCommand == command) { |
// Keep timer from overrunning. |
209,55 → 254,3 |
void RC_calibrate(void) { |
// Do nothing. |
} |
uint8_t RC_getCommand(void) { |
if (commandTimer == COMMAND_TIMER) { |
// Stick has been held long enough; command committed. |
return lastRCCommand; |
} |
// Not yet sure what the command is. |
return COMMAND_NONE; |
} |
/* |
* Command arguments on R/C: |
* 2--3--4 |
* | | + |
* 1 0 5 ^ 0 |
* | | | |
* 8--7--6 |
* |
* + <-- |
* 0 |
* |
* Not in any of these positions: 0 |
*/ |
#define ARGUMENT_THRESHOLD 70 |
#define ARGUMENT_CHANNEL_VERTICAL CH_ELEVATOR |
#define ARGUMENT_CHANNEL_HORIZONTAL CH_AILERONS |
uint8_t RC_getArgument(void) { |
if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) { |
// vertical is up |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 2; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 4; |
return 3; |
} else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) { |
// vertical is down |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 8; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 6; |
return 7; |
} else { |
// vertical is around center |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 1; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 5; |
return 0; |
} |
} |
/branches/dongfang_FC_fixedwing/rc.h |
---|
19,6 → 19,7 |
#define CH_AILERONS 1 |
#define CH_THROTTLE 2 |
#define CH_RUDDER 3 |
#define CH_MODESWITCH 4 |
#define CH_POTS 4 |
#define POT_OFFSET 120 |
/branches/dongfang_FC_fixedwing/timer2.c |
---|
1,10 → 1,11 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "eeprom.h" |
#include "rc.h" |
#include "output.h" |
#include "flight.h" |
#include "attitude.h" |
#define COARSERESOLUTION 1 |
// #define COARSERESOLUTION 1 |
#ifdef COARSERESOLUTION |
#define NEUTRAL_PULSELENGTH 938 |
26,7 → 27,6 |
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
//volatile uint8_t servoActive = 0; |
volatile uint8_t recalculateServoTimes = 0; |
volatile uint16_t servoValues[MAX_SERVOS]; |
volatile uint16_t previousManualValues[2]; |
83,28 → 83,9 |
SREG = sreg; |
} |
/* |
void servo_On(void) { |
servoActive = 1; |
} |
void servo_Off(void) { |
servoActive = 0; |
HEF4017R_ON; // enable reset |
} |
*/ |
/***************************************************** |
* Control Servo Position |
* Control (camera gimbal etc.) servos |
*****************************************************/ |
/*typedef struct { |
uint8_t manualControl; |
uint8_t compensationFactor; |
uint8_t minValue; |
uint8_t maxValue; |
uint8_t flags; |
} servo_t;*/ |
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. |
136,14 → 117,6 |
if (value < limit) value = limit; |
limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
if (value > limit) value = limit; |
return value; |
} |
uint16_t servoValue(uint8_t axis) { |
int16_t value; |
if (axis<2) value = featuredServoValue(axis); |
else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8. |
// Shift out of the [0..255*SCALE_FACTOR] space |
value -= (128 * SCALE_FACTOR); |
if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
else if (value > SERVOLIMIT) value = SERVOLIMIT; |
151,11 → 124,33 |
return value + NEUTRAL_PULSELENGTH; |
} |
void calculateServoValues(void) { |
void calculateControlServoValues(void) { |
int16_t value; |
for (uint8_t axis=0; axis<3; axis++) { |
value = controlServos[axis]; |
value /= 2; |
servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
} |
debugOut.analog[18] = servoValues[0]; |
debugOut.analog[19] = servoValues[2]; |
} |
void calculateFeaturedServoValues(void) { |
int16_t value; |
uint8_t axis; |
// Save the computation cost of computing a new value before the old one is used. |
if (!recalculateServoTimes) return; |
for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
servoValues[axis] = servoValue(axis); |
for (axis=0; axis<2; axis++) { |
value = featuredServoValue(axis); |
servoValues[axis + 3] = value; |
} |
for (axis=2; axis<MAX_SERVOS; axis++) { |
value = 128 * SCALE_FACTOR; |
servoValues[axis + 3] = value; |
} |
recalculateServoTimes = 0; |
} |
/branches/dongfang_FC_fixedwing/timer2.h |
---|
6,6 → 6,9 |
extern volatile int16_t ServoNickValue; |
extern volatile int16_t ServoRollValue; |
void calculateControlServoValues(void); |
void calculateFeaturedServoValues(void); |
void timer2_init(void); |
#endif //_TIMER2_H |
/branches/dongfang_FC_fixedwing/uart0.c |
---|
81,26 → 81,26 |
// keep lables in flash to save 512 bytes of sram space |
const prog_uint8_t ANALOG_LABEL[32][16] = { |
//1234567890123456 |
"AnglePitch ", //0 |
"AngleRoll ", |
"AngleYaw ", |
"GyroPitch ", |
"GyroRoll ", |
"GyroYaw ", //5 |
"PitchTerm ", |
"RollTerm ", |
"ThrottleTerm ", |
"YawTerm ", |
"heightP ", //10 |
"heightI ", |
"heightD ", |
"gyroActivity ", |
"ca ", |
"GActivityDivider", //15 |
"NaviMode ", |
"NaviStatus ", |
"NaviStickP ", |
"NaviStickR ", |
"Gyro P ", //0 |
"Gyro R ", |
"Gyro Y ", |
"Attitude P ", |
"Attitude R ", |
"Attitude Y ", //5 |
"Target P ", |
"Target R ", |
"Target Y ", |
"Error P ", |
"Error R ", //10 |
"Error Y ", |
"Term P ", |
"Term R ", |
"Term Y ", |
"FlightMode ", //15 |
"RC P ", |
"RC Thr ", |
"ServoFinal P ", |
"ServoFinal T ", |
"control act wghd", //20 |
"acc vector wghd ", |
"Height[dm] ", |
483,7 → 483,7 |
break; |
case 'j':// Save IMU configuration |
if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off |
if (!isMotorRunning) // save settings only if motors are off |
{ |
if ((pRxData[0] == IMUCONFIG_REVISION) && (pRxData[1] == sizeof(IMUConfig))) { |
memcpy(&IMUConfig, (uint8_t*) &pRxData[2], sizeof(IMUConfig)); |
520,7 → 520,7 |
break; |
case 's': // save settings |
if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off |
if (!isMotorRunning) // save settings only if motors are off |
{ |
if ((pRxData[1] == EEPARAM_REVISION) && (pRxData[2] == sizeof(staticParams))) // check for setting to be in range and version of settings |
{ |