Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2101 → Rev 2102

/branches/dongfang_FC_fixedwing/.cproject
1,53 → 1,210
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
 
<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">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
</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"/>
</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>
<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>
</toolChain>
</folderInfo>
</configuration>
</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"/>
</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>
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
 
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<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"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<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.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.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 &quot;${plugin_state_location}/${specs_file}&quot;'" 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 &quot;${plugin_state_location}/specs.cpp&quot;'" 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 &quot;${plugin_state_location}/specs.c&quot;'" 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 &quot;${plugin_state_location}/${specs_file}&quot;'" 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 &quot;${plugin_state_location}/specs.cpp&quot;'" 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 &quot;${plugin_state_location}/specs.c&quot;'" 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.null.1278448474" name="dongfang_FC_fixedwing"/>
</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
13,34 → 13,24
#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();
/*
* 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 (!(MKFlags & MKFLAG_MOTOR_RUN)) {
if (command == COMMAND_GYROCAL && !repeated) {
// Gyro calinbration, with or without selecting a new parameter-set.
paramSet_readFromEEProm(1);
analog_calibrateGyros();
attitude_setNeutral();
controlMixer_setNeutral();
beepNumber(1);
}
 
// 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.
// 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);
analog_calibrateGyros();
attitude_setNeutral();
controlMixer_setNeutral();
beepNumber(1);
} else if (command == COMMAND_CHMOD && !repeated) {
configuration_setFlightParameters(argument);
}
} // 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 */
40,41 → 37,38
#define CONTROL_CONFIG_SCALE 10
 
void flight_setGround(void) {
IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
target[PITCH] = attitude[PITCH];
target[ROLL] = attitude[ROLL];
target[YAW] = attitude[YAW];
IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
target[PITCH] = attitude[PITCH];
target[ROLL] = attitude[ROLL];
target[YAW] = attitude[YAW];
}
 
void switchToFlightMode(FlightMode_t flightMode) {
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...;
}
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];
}
// this should be followed by a call to switchToFlightMode!!
void updateFlightParametersToFlightMode(uint8_t flightMode) {
debugOut.analog[15] = flightMode;
 
dFactor[PITCH] = staticParams.gyroPitchD / CONTROL_CONFIG_SCALE;
dFactor[ROLL] = staticParams.gyroRollD / CONTROL_CONFIG_SCALE;
dFactor[YAW] = staticParams.gyroYawD / CONTROL_CONFIG_SCALE;
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;
 
// TODO: Set reverse also.
for (uint8_t i = 0; i < 3; i++) {
if (flightMode == FLIGHT_MODE_MANUAL) {
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_ANGLES) {
iFactor[i] = staticParams.gyroPID[i].I;
} else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) {
iFactor[i] = 0;
}
}
}
 
/************************************************************************/
81,120 → 75,130
/* Main Flight Control */
/************************************************************************/
void flight_control(void) {
// Mixer Fractions that are combined for Motor Control
int16_t throttleTerm, term[3];
// Mixer Fractions that are combined for Motor Control
int16_t throttleTerm, term[3];
 
// PID controller variables
int16_t PDPart[3];
// PID controller variables
int16_t PDPart[3];
 
static int8_t debugDataTimer = 1;
static int8_t debugDataTimer = 1;
 
// High resolution motor values for smoothing of PID motor outputs
// static int16_t outputFilters[MAX_OUTPUTS];
// High resolution motor values for smoothing of PID motor outputs
// static int16_t outputFilters[MAX_OUTPUTS];
 
uint8_t axis;
uint8_t axis;
 
// TODO: Check modern version.
// calculateFlightAttitude();
// TODO: Check modern version.
// controlMixer_update();
throttleTerm = controls[CONTROL_THROTTLE];
// TODO: Check modern version.
// calculateFlightAttitude();
// TODO: Check modern version.
// controlMixer_update();
throttleTerm = controls[CONTROL_THROTTLE];
 
// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator;
target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons;
target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder;
// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator;
target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons;
target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder;
 
for (axis = PITCH; axis <= YAW; axis++) {
if (target[axis] > OVER180) {
target[axis] -= OVER360;
} else if (attitude[axis] <= -OVER180) {
attitude[axis] += OVER360;
}
for (axis = PITCH; axis <= YAW; axis++) {
if (target[axis] > OVER180) {
target[axis] -= OVER360;
} else if (attitude[axis] <= -OVER180) {
attitude[axis] += OVER360;
}
 
if (reverse[axis])
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;
}
if (reverse[axis])
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;
}
 
/************************************************************************/
/* Calculate control feedback from angle (gyro integral) */
/* and angular velocity (gyro signal) */
/************************************************************************/
PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6)
+ ((differential[axis] * (int16_t) dFactor[axis]) >> 4);
if (reverse[axis])
PDPart[axis] = -PDPart[axis];
/************************************************************************/
/* Calculate control feedback from angle (gyro integral) */
/* and angular velocity (gyro signal) */
/************************************************************************/
PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6)
+ ((differential[axis] * (int16_t) dFactor[axis]) >> 4);
if (reverse[axis])
PDPart[axis] = -PDPart[axis];
 
int16_t anglePart = (error[axis] * iFactor[axis]) >> 10;
if (reverse[axis])
PDPart[axis] += anglePart;
else
PDPart[axis] -= anglePart;
int16_t anglePart = (error[axis] * iFactor[axis]) >> 10;
if (reverse[axis])
PDPart[axis] += anglePart;
else
PDPart[axis] -= anglePart;
 
// Add I parts here... these are integrated errors.
// When an error wraps, actually its I part should be negated or something...
// Add I parts here... these are integrated errors.
// When an error wraps, actually its I part should be negated or something...
 
term[axis] = controls[axis] + PDPart[axis] + IPart[axis];
}
term[axis] = controls[axis] + PDPart[axis] + IPart[axis];
}
 
debugOut.analog[12] = term[PITCH];
debugOut.analog[13] = term[ROLL];
debugOut.analog[14] = throttleTerm;
debugOut.analog[15] = term[YAW];
debugOut.analog[12] = term[PITCH];
debugOut.analog[13] = term[ROLL];
debugOut.analog[14] = throttleTerm;
debugOut.analog[15] = term[YAW];
 
for (uint8_t i = 0; i < MAX_SERVOS; i++) {
int16_t tmp;
if (servoTestActive) {
servos[i] = ((int16_t) servoTest[i] - 128) * 4;
} else {
// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
switch (i) {
case 0:
tmp = term[ROLL];
break;
case 1:
tmp = term[PITCH];
break;
case 2:
tmp = throttleTerm;
break;
case 3:
tmp = term[YAW];
break;
default:
tmp = 0;
}
// These are all signed and in the same units as the RC stuff in rc.c.
servos[i] = tmp;
}
}
for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) {
int16_t tmp;
if (servoTestActive) {
controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;
} else {
// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
switch (i) {
case 0:
tmp = term[ROLL];
break;
case 1:
tmp = term[PITCH];
break;
case 2:
tmp = throttleTerm;
break;
case 3:
tmp = term[YAW];
break;
default:
tmp = 0;
}
// These are all signed and in the same units as the RC stuff in rc.c.
controlServos[i] = tmp;
}
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 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);
calculateControlServoValues;
 
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);
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugging
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if (!(--debugDataTimer)) {
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
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[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[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[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
//debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg
}
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
//debugOut.analog[23] = (10 * IPart[ROLL]) / 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
{