/branches/dongfang_FC_rewrite/analog.c |
---|
11,13 → 11,10 |
// for Delay functions |
#include "timer0.h" |
// For debugOut |
#include "uart0.h" |
// For reading and writing acc. meter offsets. |
#include "eeprom.h" |
// For debugOut.digital |
// For debugOut |
#include "output.h" |
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
492,7 → 489,9 |
analog_updateAccelerometers(); |
analog_updateAirPressure(); |
analog_updateBatteryVoltage(); |
#ifdef USE_MK3MAG |
debugOut.analog[12] = magneticHeading = volatileMagneticHeading; |
#endif |
} |
void analog_setNeutral() { |
/branches/dongfang_FC_rewrite/attitude.c |
---|
119,7 → 119,9 |
// reset gyro integrals to acc guessing |
setStaticAttitudeAngles(); |
#ifdef USE_MK3MAG |
attitude_resetHeadingToMagnetic(); |
#endif |
// Servo_On(); //enable servo output |
} |
331,12 → 333,13 |
//debugOut.analog[18] = accVector; |
} |
#ifdef USE_MK3MAG |
void attitude_resetHeadingToMagnetic(void) { |
if (commands_isCalibratingCompass()) |
return; |
// Compass is off, skip. |
if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED)) |
return; |
// Compass is invalid, skip. |
408,6 → 411,7 |
// headingError += correction; |
debugOut.analog[29] = 0; |
} |
#endif |
/************************************************************************ |
* Main procedure. |
426,9 → 430,11 |
// Interrupt-driven sensor reading may restart. |
startAnalogConversionCycle(); |
if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
#ifdef USE_MK3MAG |
if (staticParams.bitConfig & (CFG_COMPASS_ENABLED | CFG_GPS_ENABLED)) { |
correctHeadingToMagnetic(); |
} |
#endif |
} |
/* |
/branches/dongfang_FC_rewrite/attitude.h |
---|
8,10 → 8,8 |
#include <inttypes.h> |
#include "analog.h" |
#include "timer0.h" |
// For debugging only. |
#include "uart0.h" |
/* |
* If you have no acc. sensor or do not want to use it, remove this define. This will cause the |
* acc. sensor to be ignored at attitude calibration. |
27,7 → 25,7 |
/* |
* The frequency at which numerical integration takes place. 488 in original code. |
*/ |
#define INTEGRATION_FREQUENCY 488 |
#define INTEGRATION_FREQUENCY F_MAINLOOP |
/* |
* Gyro readings are divided by this before being used in attitude control. This will scale them |
59,6 → 57,7 |
FC2.0: GYRO_DEG_FACTOR_PITCHROLL = 2399 |
My InvenSense copter: GYRO_DEG_FACTOR_PITCHROLL = 1333 |
*/ |
//#define GYRO_PITCHROLL_CORRECTION GYRO_PITCHROLL_CORRECTION_should_be_overridden_with_a_-D_at_compile_time |
#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR) |
#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION) |
/branches/dongfang_FC_rewrite/attitudeControl.c |
---|
1,6 → 1,5 |
#include <inttypes.h> |
#include "attitude.h" |
#include "uart0.h" |
#include "configuration.h" |
#include "dongfangMath.h" |
#include "controlMixer.h" |
/branches/dongfang_FC_rewrite/commands.c |
---|
56,8 → 56,12 |
#include "eeprom.h" |
#include "attitude.h" |
#include "output.h" |
#include "rc.h" |
#ifdef USE_MK3MAG |
// TODO: Kick that all outa here! |
uint8_t compassCalState = 0; |
#endif |
void commands_handleCommands(void) { |
/* |
88,11 → 92,14 |
flight_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(getActiveParamSet()); |
} else if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && argument == 7) { |
} |
#ifdef USE_MK3MAG |
else if (staticParams.bitConfig & (CFG_COMPASS_ENABLED | CFG_GPS_ENABLED) && argument == 7) { |
// If right stick is centered and down |
compassCalState = 1; |
beep(1000); |
} |
#endif |
} |
// save the ACC neutral setting to eeprom |
135,6 → 142,8 |
* |
*/ |
#ifdef USE_MK3MAG |
uint8_t commands_isCalibratingCompass(void) { |
return RC_testCompassCalState(); |
} |
#endif |
/branches/dongfang_FC_rewrite/commands.h |
---|
15,6 → 15,9 |
extern uint8_t compassCalState; |
void commands_handleCommands(void); |
#ifdef USE_MK3MAG |
uint8_t commands_isCalibratingCompass(void); |
#endif |
#endif |
/branches/dongfang_FC_rewrite/compassControl.c |
---|
1,19 → 1,66 |
#include <inttypes.h> |
#include "controlMixer.h" |
#include "attitude.h" |
#include "compassControl.h" |
#include <stdlib.h> |
// The only possibility in a compass control is to aim the head at some compass heading. |
// One way could be: When a switch is turned on, we capture the current heading in a variable. |
// From then on, if the current heading is to the right of the captured heading, we yaw left etc. |
// If the yaw at input is nonzero, we could also temporarily disable this and instead re-capture |
// the current heading.. |
// 4 modes of control (like with the simple height controller): |
// 0) Off: Normal yaw control, supported by compass (anti drift) if present and enabled |
// 1) Heading is captured at takeoff, and held there (plus / minus bending via remote) |
// 2) A variable controls the heading (plus / minus bending via remote) |
// 3) Navigation controls the heading (plus / minus bending via remote) |
// The bending should be linear, or rotation rate controlled. |
// The target heading variable is stored here (not in the flight or attitude modules), as here |
// is where the regulation takes place. Also, it was found that a target heading was difficult |
// to maintain in the flight module and the flight module would sometimes need to write back |
// to it. The only possible (but sufficient) output from here is PRTY[CONTROL_YAW]. |
int32_t navigationTargetHeading; |
int32_t magneticTargetHeading; |
int32_t bending = 0; |
void compass_setTakeoffHeading(int32_t heading) { |
magneticTargetHeading = heading; |
} |
void CC_periodicTaskAndPRTY(int16_t* PRTY) { |
int16_t currentYaw = PRTY[CONTROL_YAW]; |
if (abs(currentYaw) < 10) { |
magneticTargetHeading = magneticHeading; |
switch (staticParams.compassMode) { |
case COMPASS_MODE_OFF: |
bending = 0; |
break; |
case COMPASS_MODE_TAKEOFF: |
// just leave the target heading untouched! It should have been set at takeoff. |
break; |
// case COMPASS_MODE_RCVARIABLE: |
// magneticTargetHeading = (int32_t)dynamicParams.compassControlHeading * (360L * GYRO_DEG_FACTOR_YAW >> 8); |
// break; |
case COMPASS_MODE_NAVIGATION: |
magneticTargetHeading = navigationTargetHeading; |
break; |
} |
if (staticParams.compassMode != COMPASS_MODE_OFF) { |
if (abs(currentYaw) >= staticParams.naviStickThreshold) { |
// Bending: We do not actually need to do anything here, as the stick movement behing the bending will |
// pass right thru and do its stuff in the flight module. |
// All we have to do is to not counteract bending, and to return somewhat gracefully to target heading |
// again (using what parameter as speed?) when pilot stops bending. |
bending += currentYaw; |
} else { |
if (bending > staticParams.compassBendingReturnSpeed) { |
bending -= staticParams.compassBendingReturnSpeed; |
} else if (bending < -staticParams.compassBendingReturnSpeed) { |
bending += staticParams.compassBendingReturnSpeed; |
} else |
bending = 0; |
} |
// We have to output something proportional to the difference between magneticTargetHeading and heading (or a full PID). |
// Bending blends in like: (magneticTargetHeading +- bending - heading) |
} |
} |
/branches/dongfang_FC_rewrite/compassControl.h |
---|
3,8 → 3,21 |
#include <inttypes.h> |
extern int32_t magneticTargetHeading; |
// 4 modes of control (like with the simple height controller): |
// 0) Off: Normal yaw control, supported by compass (anti drift) if present and enabled |
// 1) Heading is captured at takeoff, and held there (plus / minus bending via remote) |
// 2) A variable controls the heading (plus / minus bending via remote) (not implemented, is it useful at all?) |
// 3) Navigation controls the heading (plus / minus bending via remote) |
#define COMPASS_MODE_OFF 0 |
#define COMPASS_MODE_TAKEOFF 1 |
//#define COMPASS_MODE_RCVARIABLE 2 |
#define COMPASS_MODE_NAVIGATION 3 |
// public read write. |
extern int32_t navigationTargetHeading; |
void compass_setTakeoffHeading(int32_t heading); |
void CC_periodicTaskAndPRTY(int16_t* PRTY); |
#endif |
/branches/dongfang_FC_rewrite/configuration.c |
---|
55,7 → 55,6 |
#include "configuration.h" |
#include "sensors.h" |
#include "rc.h" |
//#include "uart0.h" |
#include "output.h" |
int16_t variables[VARIABLE_COUNT]; |
87,7 → 86,7 |
SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200); |
SET_POT(dynamicParams.gyroI, staticParams.gyroI); |
SET_POT(dynamicParams.gyroD, staticParams.gyroD); |
SET_POT(dynamicParams.compassFixedHeading,staticParams.compassFixedHeading); |
//SET_POT(dynamicParams.compassControlHeading,staticParams.compassControlHeading); |
SET_POT(dynamicParams.externalControl, staticParams.externalControl); |
SET_POT(dynamicParams.dynamicStability,staticParams.dynamicStability); |
251,7 → 250,7 |
staticParams.IFactor = 32; |
staticParams.yawIFactor = 100; |
staticParams.compassYawCorrection = 64; |
staticParams.compassFixedHeading = 0; |
staticParams.compassP = 50; |
staticParams.levelCorrection[0] = staticParams.levelCorrection[1] = 128; |
// Servos |
294,7 → 293,7 |
} |
staticParams.bitConfig = |
CFG_GYRO_SATURATION_PREVENTION | CFG_HEADING_HOLD | CFG_COMPASS_ACTIVE; |
CFG_GYRO_SATURATION_PREVENTION | CFG_COMPASS_ENABLED; |
memcpy(staticParams.name, "Default\0", 6); |
} |
/branches/dongfang_FC_rewrite/configuration.h |
---|
42,7 → 42,7 |
/*PMM*/uint8_t gyroP; |
/* P */uint8_t gyroI; |
/* P */uint8_t gyroD; |
/* P */uint8_t compassFixedHeading; |
/* P */uint8_t compassControlHeading; |
// Control |
/* P */uint8_t externalControl; |
162,8 → 162,10 |
uint8_t IFactor; |
uint8_t yawIFactor; |
uint8_t compassMode; // bitflag thing. |
uint8_t compassYawCorrection; |
uint8_t compassFixedHeading; |
uint8_t compassBendingReturnSpeed; |
uint8_t compassP; |
uint8_t batteryVoltageWarning; |
uint8_t emergencyThrottle; |
223,9 → 225,9 |
#define CFG_SIMPLE_HEIGHT_CONTROL (1<<0) |
#define CFG_SIMPLE_HC_HOLD_SWITCH (1<<1) |
#define CFG_HEADING_HOLD (1<<2) |
#define CFG_COMPASS_ACTIVE (1<<3) |
#define CFG_UNUSED (1<<4) |
#define CFG_GPS_ACTIVE (1<<5) |
#define CFG_COMPASS_ENABLED (1<<3) |
#define CFG_UNUSED (1<<4) |
#define CFG_GPS_ENABLED (1<<5) |
#define CFG_AXIS_COUPLING_ACTIVE (1<<6) |
#define CFG_GYRO_SATURATION_PREVENTION (1<<7) |
/branches/dongfang_FC_rewrite/controlMixer.c |
---|
157,8 → 157,10 |
// Add external control to RC |
EC_periodicTaskAndPRTY(tempPRTY); |
#ifdef USE_DIRECT_GPS |
// Add navigations control to RC and external control. |
navigation_periodicTaskAndPRTY(tempPRTY); |
#endif |
// Add compass control (could also have been before navi, they are independent) |
CC_periodicTaskAndPRTY(tempPRTY); |
/branches/dongfang_FC_rewrite/directGPSNaviControl.c |
---|
46,16 → 46,15 |
void navi_updateFlightMode(void) { |
static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF; |
if (controlMixer_getSignalQuality() <= SIGNAL_BAD |
|| MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
if (controlMixer_getSignalQuality() <= SIGNAL_BAD || (MKFlags & MKFLAG_EMERGENCY_FLIGHT)) { |
flightMode = GPS_FLIGHT_MODE_HOME; |
} else { |
if (dynamicParams.naviMode < 50) |
flightMode = GPS_FLIGHT_MODE_HOME; |
flightMode = GPS_FLIGHT_MODE_FREE; |
else if (dynamicParams.naviMode < 180) |
flightMode = GPS_FLIGHT_MODE_AID; |
else |
flightMode = GPS_FLIGHT_MODE_FREE; |
flightMode = GPS_FLIGHT_MODE_HOME; |
} |
if (flightMode != flightModeOld) { |
/branches/dongfang_FC_rewrite/dongfangMath.c |
---|
5,9 → 5,6 |
// For scope debugging only! |
#include "output.h" |
// Debug |
#include "uart0.h" |
const int16_t SIN_TABLE[] PROGMEM = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5), |
/branches/dongfang_FC_rewrite/flight.c |
---|
54,14 → 54,20 |
#include "eeprom.h" |
#include "flight.h" |
#include "output.h" |
#include "uart0.h" |
// Necessary for external control and motor test |
#include "uart0.h" |
#include "twimaster.h" |
#include "attitude.h" |
#include "controlMixer.h" |
#include "commands.h" |
#include "heightControl.h" |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#include "compassControl.h" |
#endif |
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
/* |
147,7 → 153,7 |
// PID controller variables |
int16_t PDPart[2],/* DPart[2],*/ PDPartYaw /*, DPartYaw */; |
static int32_t IPart[2] = { 0, 0 }; |
static int32_t IPart[2] = {0, 0}; |
static uint16_t emergencyFlightTime; |
static int8_t debugDataTimer = 1; |
171,7 → 177,7 |
/************************************************************************/ |
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
if (controlMixer_didReceiveSignal) beepRCAlarm(); |
if (controlMixer_didReceiveSignal) beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
if (emergencyFlightTime) { |
// continue emergency flight |
emergencyFlightTime--; |
193,7 → 199,10 |
// Reset emergency landing control variables. |
MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing |
// The time is in whole seconds. |
emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 488; |
if (staticParams.emergencyFlightDuration > (65535-F_MAINLOOP)/F_MAINLOOP) |
emergencyFlightTime = 0xffff; |
else |
emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * F_MAINLOOP; |
} |
// If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
211,8 → 220,11 |
IPart[PITCH] = IPart[ROLL] = 0; |
PDPartYaw = 0; |
if (isFlying == 250) { |
// HC_setGround(); |
HC_setGround(); |
#ifdef USE_MK3MAG |
attitude_resetHeadingToMagnetic(); |
compass_setTakeoffHeading(heading); |
#endif |
// Set target heading to the one just gotten off compass. |
// targetHeading = heading; |
} |
226,24 → 238,11 |
setNormalFlightParameters(); |
} // end else (not bad signal case) |
/************************************************************************/ |
/* Yawing */ |
/************************************************************************/ |
/* |
if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated |
ignoreCompassTimer = 1000; |
if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) { |
//targetHeading = heading; |
// YGBSM!!! |
} |
} |
*/ |
#if defined (USE_NAVICTRL) |
/************************************************************************/ |
/* GPS is currently not supported. */ |
/************************************************************************/ |
if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
if(staticParams.GlobalConfig & CFG_GPS_ENABLED) { |
GPS_Main(); |
MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
} else { |
261,7 → 260,15 |
PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral |
PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5); |
PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
CHECK_MIN_MAX(PDPart[axis], -6L*GYRO_DEG_FACTOR_PITCHROLL, 6L*GYRO_DEG_FACTOR_PITCHROLL); |
//CHECK_MIN_MAX(PDPart[axis], -6L*GYRO_DEG_FACTOR_PITCHROLL, 6L*GYRO_DEG_FACTOR_PITCHROLL); |
if (PDPart[axis] < -6L*GYRO_DEG_FACTOR_PITCHROLL) { |
PDPart[axis] =- 6L*GYRO_DEG_FACTOR_PITCHROLL; |
debugOut.digital[0] |= DEBUG_FLIGHTCLIP; |
} else if (PDPart[axis] > 6L*GYRO_DEG_FACTOR_PITCHROLL) { |
PDPart[axis] = 6L*GYRO_DEG_FACTOR_PITCHROLL; |
debugOut.digital[0] |= DEBUG_FLIGHTCLIP; |
} |
} |
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW) |
350,8 → 357,15 |
tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability |
* (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
// TODO: From which planet comes the 16000? |
CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
//CHECK_MIN_MAX(IPart[axis], -25L*GYRO_DEG_FACTOR_PITCHROLL, 25L*GYRO_DEG_FACTOR_PITCHROLL); |
if (IPart[axis] < -25L*GYRO_DEG_FACTOR_PITCHROLL) { |
IPart[axis] =- 25L*GYRO_DEG_FACTOR_PITCHROLL; |
debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
} else if (PDPart[axis] > 25L*GYRO_DEG_FACTOR_PITCHROLL) { |
PDPart[axis] = 25L*GYRO_DEG_FACTOR_PITCHROLL; |
debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
} |
// Add (P, D) parts minus stick pos. to the scaled-down I part. |
term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
/branches/dongfang_FC_rewrite/heightControl.c |
---|
1,7 → 1,6 |
#include <inttypes.h> |
#include "analog.h" |
#include "attitude.h" |
#include "uart0.h" |
#include "configuration.h" |
#include "controlMixer.h" |
/branches/dongfang_FC_rewrite/main.c |
---|
72,7 → 72,7 |
#ifdef USE_NAVICTRL |
#include "spi.h" |
#endif |
#ifdef USE_DIRECT_GPS |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
#include "eeprom.h" |
108,8 → 108,10 |
#ifdef USE_NAVICTRL |
SPI_MasterInit(); |
#endif |
#ifdef USE_MK3MAG |
MK3MAG_init(); |
#endif |
#ifdef USE_DIRECT_GPS |
MK3MAG_init(); |
usart1_init(); |
#endif |
/branches/dongfang_FC_rewrite/makefile |
---|
16,7 → 16,8 |
# Use one of the extensions for a gps solution |
#EXT = NAVICTRL |
EXT = DIRECT_GPS |
#EXT = DIRECT_GPS |
EXT = MK3MAG |
#EXT = |
#GYRO=ENC-03_FC1.3 |
136,6 → 137,10 |
SRC += mk3mag.c directGPSNaviControl.c uart1.c ubx.c |
endif |
ifeq ($(EXT), MK3MAG) |
SRC += mk3mag.c |
endif |
ifeq ($(EXT), NAVICTRL) |
SRC += spi.c |
endif |
182,7 → 187,13 |
ifeq ($(EXT), DIRECT_GPS) |
CFLAGS += -DUSE_DIRECT_GPS |
CFLAGS += -DUSE_MK3MAG |
endif |
ifeq ($(EXT), MK3MAG) |
CFLAGS += -DUSE_MK3MAG |
endif |
ifeq ($(EXT), NAVICTRL) |
CFLAGS += -DUSE_NAVICTRL |
endif |
/branches/dongfang_FC_rewrite/mk3mag.c |
---|
2,6 → 2,7 |
#include <stdlib.h> |
#include <inttypes.h> |
#include "timer0.h" |
#include "output.h" |
#include "eeprom.h" |
#include "mk3mag.h" |
/branches/dongfang_FC_rewrite/output.c |
---|
1,62 → 1,10 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 04.2007 Holger Buss |
// + Nur für den privaten Gebrauch |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <inttypes.h> |
#include "output.h" |
#include "eeprom.h" |
#include "timer0.h" |
// To access the DebugOut struct. |
#include "uart0.h" |
uint8_t flashCnt[2], flashMask[2]; |
// initializes the LED control outputs J16, J17 |
DebugOut_t debugOut; |
void output_init(void) { |
// set PC2 & PC3 as output (control of J16 & J17) |
/branches/dongfang_FC_rewrite/output.h |
---|
46,7 → 46,7 |
#define DEBUG_MAINLOOP_TIMER 1 |
#define DEBUG_HEIGHT_DIFF 2 |
#define DEBUG_HOVERTHROTTLE 4 |
#define DEBUG_FLIGHTCLIP 4 |
#define DEBUG_ACC0THORDER 8 |
#define DEBUG_COMPASS 16 |
#define DEBUG_PRESSURERANGE 32 |
61,6 → 61,14 |
#define OUTPUTFLAGS_TEST_OFF 32 // For testing: Turn off both outputs |
#define OUTPUTFLAGS_TEST_ON 64 // For testing: Turn on both outputs |
// For practical reasons put here instead of in uart0.h |
typedef struct { |
uint8_t digital[2]; |
uint16_t analog[32]; // debug values |
}__attribute__((packed)) DebugOut_t; |
extern DebugOut_t debugOut; |
/* |
* Set to 0 for using outputs as the usual flashing lights. |
* Set to one of the DEBUG_... defines h for using the outputs as debug lights. |
/branches/dongfang_FC_rewrite/rc.c |
---|
3,10 → 3,10 |
#include <avr/interrupt.h> |
#include "rc.h" |
#include "uart0.h" |
#include "controlMixer.h" |
#include "configuration.h" |
#include "commands.h" |
#include "output.h" |
// The channel array is 0-based! |
volatile int16_t PPM_in[MAX_CHANNELS]; |
190,7 → 190,7 |
if (PRTY[CONTROL_THROTTLE] < 0) PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
// exponential stick sensitivity in yawing rate |
tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
tmp2 = (int32_t)staticParams.stickYawP * ((int32_t)tmp1 * abs(tmp1)) >> 9; // expo y = ax + bx^2 |
tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
PRTY[CONTROL_YAW] = tmp2; |
308,6 → 308,7 |
} |
} |
#ifdef USE_MK3MAG |
/* |
* For each time the stick is pulled, returns true. |
*/ |
323,6 → 324,8 |
} |
return 0; |
} |
#endif |
/* |
* Abstract controls are not used at the moment. |
t_control rc_control = { |
/branches/dongfang_FC_rewrite/rc.h |
---|
38,5 → 38,7 |
void RC_calibrate(void); |
uint8_t RC_getSignalQuality(void); |
uint8_t RC_getLooping(uint8_t looping); |
#ifdef USE_MK3MAG |
uint8_t RC_testCompassCalState(void); |
#endif |
#endif //_RC_H |
/branches/dongfang_FC_rewrite/spi.c |
---|
55,11 → 55,9 |
#include "spi.h" |
#include "rc.h" |
#include "eeprom.h" |
#include "uart0.h" |
#include "timer0.h" |
#include "analog.h" |
#include "attitude.h" |
#include "GPSControl.h" |
#include "flight.h" |
//----------------------------------------- |
/branches/dongfang_FC_rewrite/timer0.c |
---|
56,11 → 56,9 |
#include "controlMixer.h" |
#include "timer0.h" |
// for debugging! |
#include "uart0.h" |
#include "output.h" |
#ifdef USE_DIRECT_GPS |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
183,12 → 181,12 |
PORTC &= ~(1 << PORTC7);// Speaker at PC7 |
} |
#ifdef USE_DIRECT_GPS |
#ifdef USE_MK3MAG |
// update compass value if this option is enabled in the settings |
if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
if (staticParams.bitConfig & (CFG_COMPASS_ENABLED | CFG_GPS_ENABLED)) { |
MK3MAG_periodicTask(); // read out mk3mag pwm |
} |
#endif |
} |
} |
// ----------------------------------------------------------------------- |
/branches/dongfang_FC_rewrite/timer0.h |
---|
3,6 → 3,12 |
#include <inttypes.h> |
// Normally it is 20MHz/2048 = 9765.625 Hz |
#define F_TIMER0IRQ ((float)F_CPU/2048.0) |
#define MAINLOOP_DIVIDER 10 |
// Originally it is 488 |
#define F_MAINLOOP (F_TIMER0IRQ/(2.0*MAINLOOP_DIVIDER)) |
#define BEEP_MODULATION_NONE 0xFFFF |
#define BEEP_MODULATION_RCALARM 0x0C00 |
#define BEEP_MODULATION_I2CALARM 0x0080 |
/branches/dongfang_FC_rewrite/timer2.c |
---|
51,7 → 51,6 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "eeprom.h" |
#include "uart0.h" |
#include "rc.h" |
#include "attitude.h" |
/branches/dongfang_FC_rewrite/uart0.c |
---|
55,7 → 55,6 |
int16_t heading; |
}__attribute__((packed)) Heading_t; |
DebugOut_t debugOut; |
Data3D_t data3D; |
uint16_t debugData_timer; |
/branches/dongfang_FC_rewrite/uart0.h |
---|
20,13 → 20,6 |
extern uint8_t motorTest[16]; |
typedef struct { |
uint8_t digital[2]; |
uint16_t analog[32]; // debug values |
}__attribute__((packed)) DebugOut_t; |
extern DebugOut_t debugOut; |
typedef struct { |
int16_t anglePitch; // in 0.1 deg |
int16_t angleRoll; // in 0.1 deg |
int16_t heading; // in 0.1 deg |