Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2051 → Rev 2052

/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