WebSVN - FlightCtrl - Diff - Rev 2073 and 2088 - /branches/dongfang_FC


 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
#include <util/delay.h>
#include <stddef.h>
#include <string.h>
#include "configuration.h"
#include "sensors.h"
#include "rc.h"
#include "output.h"
#include "flight.h"
 
int16_t variables[VARIABLE_COUNT];
ParamSet_t staticParams;
channelMap_t channelMap;
mixerMatrix_t mixerMatrix;
volatile DynamicParams_t dynamicParams;
 
uint8_t CPUType;
uint8_t boardRelease;
uint8_t requiredMotors;
 
VersionInfo_t versionInfo;
 
// MK flags. TODO: Replace by enum. State machine.
uint16_t isFlying = 0;
volatile uint8_t MKFlags = 0;
 
const MMXLATION XLATIONS[] = {
{offsetof(ParamSet_t, levelCorrection[0]), offsetof(DynamicParams_t, levelCorrection[0]),0,255},
{offsetof(ParamSet_t, levelCorrection[1]), offsetof(DynamicParams_t, levelCorrection[1]),0,255},
{offsetof(ParamSet_t, gyroP), offsetof(DynamicParams_t, gyroP),0,255},
{offsetof(ParamSet_t, gyroI), offsetof(DynamicParams_t, gyroI),0,255},
{offsetof(ParamSet_t, gyroD), offsetof(DynamicParams_t, gyroD),0,255},
{offsetof(ParamSet_t, attitudeControl), offsetof(DynamicParams_t, attitudeControl),0,255},
{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255},
{offsetof(ParamSet_t, dynamicStability), offsetof(DynamicParams_t, dynamicStability),0,255},
{offsetof(ParamSet_t, heightP), offsetof(DynamicParams_t, heightP),0,255},
{offsetof(ParamSet_t, heightI), offsetof(DynamicParams_t, heightI),0,255},
{offsetof(ParamSet_t, heightD), offsetof(DynamicParams_t, heightD),0,255},
{offsetof(ParamSet_t, heightSetting), offsetof(DynamicParams_t, heightSetting),0,255},
{offsetof(ParamSet_t, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255},
{offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),0,255},
{offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255},
{offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255},
{offsetof(ParamSet_t, naviMode), offsetof(DynamicParams_t, naviMode),0,255}};
 
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {
  uint8_t result;
  if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)];
  else result = src;
  if (result < min) result = min;
  else if (result > max) result = max;
  return result;
}
 
void configuration_applyVariablesToParams(void) {
  uint8_t i, src;
  uint8_t* pointerToTgt;
 
  for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) {
    src = *((uint8_t*)(&staticParams) + XLATIONS[i].sourceIdx);
    pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx;
    *pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max);
  }
 
  // User parameters are always variable.
  for (i=0; i<sizeof(staticParams.userParams); i++) {
    src = *((uint8_t*)(&staticParams) + offsetof(ParamSet_t, userParams) + i);
    pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i;
    *pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
  }
}
 
 
void setCPUType(void) {   // works only after reset or power on when the registers have default values
  if((UCSR1A == 0x20) && (UCSR1C == 0x06)) CPUType = ATMEGA644P;  // initial Values for 644P after reset
  else CPUType = ATMEGA644;
}
 
/*
 * Automatic detection of hardware components is not supported in this development-oriented
 * FC firmware. It would go against the point of it: To enable alternative hardware
 * configurations with otherwise unsupported components. Instead, one should write
 * custom code + adjust constants for the new hardware, and include the relevant code
 * from the makefile.
 * However - we still do detect the board release. Reason: Otherwise it would be too
 * tedious to have to modify the code for how to turn on and off LEDs when deploying
 * on different HW version....
 */
void setBoardRelease(void) {
 
  // the board release is coded via the pull up or down the 2 status LED
 
  PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
  DDRB  &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input
 
  _delay_loop_2(1000); // make some delay
 
  switch( PINB & ((1<<PINB1)|(1<<PINB0))) {
    case 0x00:
      boardRelease = 10; // 1.0
      break;
    case 0x01:
      boardRelease = 11; // 1.1 or 1.2
      break;
    case 0x02:
      boardRelease = 20; // 2.0
      break;
    case 0x03:
      boardRelease = 13; // 1.3
      break;
    default:
      break;
    }
  // set LED ports as output
  DDRB |= (1<<DDB1)|(1<<DDB0);
  RED_OFF;
  GRN_OFF;
 
}
 
void configuration_setNormalFlightParameters(void) {
  flight_setParameters(staticParams.IFactor, dynamicParams.gyroP,
      staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
      dynamicParams.gyroP, staticParams.yawIFactor);
}
 
void configuration_setFailsafeFlightParameters(void) {
  flight_setParameters(0, 90, 120, 90, 120);
}
 
// 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_setNormalFlightParameters();
  // Immediately load changes to output, and also signal the paramset change.
  output_init();
}
 
void setOtherDefaults(void) {
  // Height Control
  staticParams.airpressureFilterConstant = 8;
  //staticParams.airpressureWindowLength = 0;
  staticParams.airpressureAccZCorrection = 128+56;
  staticParams.heightP = 10;
  staticParams.heightD = 30;
  staticParams.heightSetting = 251;
  staticParams.heightControlMaxThrottleChange = 10;
 
  // Control
  staticParams.stickP = 8;
  staticParams.stickD = 12;
  staticParams.stickYawP = 12;
  staticParams.stickThrottleD = 12;
  staticParams.minThrottle = 8;
  staticParams.maxThrottle = 230;
  staticParams.externalControl = 0;
  staticParams.motorSmoothing = 0;
 
  // IMU
  staticParams.gyroPIDFilterConstant = 1;
  staticParams.gyroATTFilterConstant = 1;
  staticParams.gyroDFilterConstant = 1;
  staticParams.accFilterConstant = 10;
 
  staticParams.gyroP = 60;
  staticParams.gyroI = 80;
  staticParams.gyroD = 4;
 
  // set by gyro-specific code: gyro_setDefaults().
  // staticParams.zerothOrderCorrection = 
  // staticParams.driftCompDivider = 
  // staticParams.driftCompLimit = 
 
  staticParams.dynamicStability = 50;
  staticParams.zerothOrderCorrectionAccTolerance = 60;
  staticParams.zerothOrderCorrectionControlTolerance = 60;
  staticParams.IFactor = 52;
  staticParams.yawIFactor = 100;  
  staticParams.compassYawCorrection = 64;
  staticParams.compassP = 50;
  staticParams.levelCorrection[0] = staticParams.levelCorrection[1] = 128;
 
  // Servos
  staticParams.servoCount = 7;
  staticParams.servoManualMaxSpeed = 10;
  for (uint8_t i=0; i<2; i++) {
    staticParams.servoConfigurations[i].manualControl = 128;
    staticParams.servoConfigurations[i].stabilizationFactor = 0;
    staticParams.servoConfigurations[i].minValue = 32;
    staticParams.servoConfigurations[i].maxValue = 224;
    staticParams.servoConfigurations[i].flags = 0;
  }
 
  // Battery warning and emergency flight
  staticParams.batteryVoltageWarning = 101;  // 3.7 each for 3S
  staticParams.emergencyThrottle = 35;
  staticParams.emergencyFlightDuration = 30;
 
  // Outputs
  staticParams.outputFlash[0].bitmask = 1; //0b01011111;
  staticParams.outputFlash[0].timing = 15;
  staticParams.outputFlash[1].bitmask = 3; //0b11110011;
  staticParams.outputFlash[1].timing = 15;
 
  staticParams.outputDebugMask = 8;
  staticParams.outputFlags   = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS;
 
  staticParams.naviMode = 0; // free.
 
  staticParams.maxControlActivity = 0; // temporary!
  staticParams.maxAccVector = 80;
}
 
/***************************************************/
/*    Default Values for parameter set 1           */
/***************************************************/
void paramSet_default(uint8_t setnumber) {
  setOtherDefaults();
  gyro_setDefaultParameters();
 
  for (uint8_t i=0; i<8; i++) {
    staticParams.userParams[i] = 0;
  }
 
  staticParams.bitConfig =
    CFG_GYRO_SATURATION_PREVENTION | CFG_COMPASS_ENABLED;
 
  memcpy(staticParams.name, "Default\0", 6);
}
 
/***************************************************/
/*    Default Values for Mixer Table               */
/***************************************************/
void mixerMatrix_default(void) { // Quadro 
  uint8_t i;
  // mixerMatric.revision = EEMIXER_REVISION;
  // clear mixer table (but preset throttle)
  for (i = 0; i < 16; i++) {
    mixerMatrix.motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0;
    mixerMatrix.motor[i][MIX_PITCH] = 0;
    mixerMatrix.motor[i][MIX_ROLL] = 0;
    mixerMatrix.motor[i][MIX_YAW] = 0;
  }
  // default = Quadro
  mixerMatrix.motor[0][MIX_PITCH] = +64;
  mixerMatrix.motor[0][MIX_YAW] = +64;
  mixerMatrix.motor[1][MIX_PITCH] = -64;
  mixerMatrix.motor[1][MIX_YAW] = +64;
  mixerMatrix.motor[2][MIX_ROLL] = -64;
  mixerMatrix.motor[2][MIX_YAW] = -64;
  mixerMatrix.motor[3][MIX_ROLL] = +64;
  mixerMatrix.motor[3][MIX_YAW] = -64;
  memcpy(mixerMatrix.name, "Quadro\0", 7);
 
  /*
  // default = X
  mixerMatrix.motor[0][MIX_PITCH] = +45;
  mixerMatrix.motor[0][MIX_ROLL]  = +45;
  mixerMatrix.motor[0][MIX_YAW]   = +64;
 
  mixerMatrix.motor[1][MIX_PITCH] = -45;
  mixerMatrix.motor[1][MIX_ROLL]  = -45;
  mixerMatrix.motor[1][MIX_YAW]   = +64;
 
  mixerMatrix.motor[2][MIX_PITCH] = +45;
  mixerMatrix.motor[2][MIX_ROLL]  = -45;
  mixerMatrix.motor[2][MIX_YAW]   = -64;
 
  mixerMatrix.motor[3][MIX_PITCH] = -45;
  mixerMatrix.motor[3][MIX_ROLL]  = +45;
  mixerMatrix.motor[3][MIX_YAW]   = -64;
  */
 
  memcpy(mixerMatrix.name, "X\0", 7);
}
 
/***************************************************/
/*    Default Values for R/C Channels              */
/***************************************************/
void channelMap_default(void) {
  channelMap.channels[CH_PITCH]    = 1;
  channelMap.channels[CH_ROLL]     = 0;
  channelMap.channels[CH_THROTTLE] = 2;
  channelMap.channels[CH_YAW]      = 3;
  channelMap.channels[CH_POTS + 0] = 4;
  channelMap.channels[CH_POTS + 1] = 5;
  channelMap.channels[CH_POTS + 2] = 6;
  channelMap.channels[CH_POTS + 3] = 7;
}
 

Rev 2073	Rev 2088
1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	-
2	// + Copyright (c) 04.2007 Holger Buss	-
3	// + Nur f�r den privaten Gebrauch	-
4	// + www.MikroKopter.com	-
5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	-
6	// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),	-
7	// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten und nicht-kommerziellen Gebrauch zul�ssig ist.	-
8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	-
9	// + bzgl. der Nutzungsbedingungen aufzunehmen.	-
10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,	-
11	// + Verkauf von Luftbildaufnahmen, usw.	-
12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	-
13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,	-
14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen	-
15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	-
16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	-
17	// + auf anderen Webseiten oder Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	-
18	// + eindeutig als Ursprung verlinkt und genannt werden	-
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	-
20	// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion	-
21	// + Benutzung auf eigene Gefahr	-
22	// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den	-
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	-
24	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	-
25	// + mit unserer Zustimmung zul�ssig	-
26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	-
27	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen	-
28	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	-
29	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	-
30	// + this list of conditions and the following disclaimer.	-
31	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	-
32	// + from this software without specific prior written permission.	-
33	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet	-
34	// + for non-commercial use (directly or indirectly)	-
35	// + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	-
36	// + with our written permission	-
37	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	-
38	// + clearly linked as origin	-
39	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed	-
40	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	-
41	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	-
42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	-
43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	-
44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	-
45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	-
46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	-
47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN	-
48	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	-
49	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	-
50	// + POSSIBILITY OF SUCH DAMAGE.	-
51	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	-
52	#include <util/delay.h>	1	#include <util/delay.h>
53	#include <stddef.h>	2	#include <stddef.h>
54	#include <string.h>	3	#include <string.h>
55	#include "configuration.h"	4	#include "configuration.h"
56	#include "sensors.h"	5	#include "sensors.h"
57	#include "rc.h"	6	#include "rc.h"
58	#include "output.h"	7	#include "output.h"
59	#include "flight.h"	8	#include "flight.h"
60		9
61	int16_t variables[VARIABLE_COUNT];	10	int16_t variables[VARIABLE_COUNT];
62	ParamSet_t staticParams;	11	ParamSet_t staticParams;
63	channelMap_t channelMap;	12	channelMap_t channelMap;
64	mixerMatrix_t mixerMatrix;	13	mixerMatrix_t mixerMatrix;
65	volatile DynamicParams_t dynamicParams;	14	volatile DynamicParams_t dynamicParams;
66		15
67	uint8_t CPUType = ATMEGA644;	16	uint8_t CPUType;
68	uint8_t boardRelease = 13;	17	uint8_t boardRelease;
69	uint8_t requiredMotors;	18	uint8_t requiredMotors;
70		19
71	VersionInfo_t versionInfo;	20	VersionInfo_t versionInfo;
72		21
73	// MK flags. TODO: Replace by enum. State machine.	22	// MK flags. TODO: Replace by enum. State machine.
74	uint16_t isFlying = 0;	23	uint16_t isFlying = 0;
75	volatile uint8_t MKFlags = 0;	24	volatile uint8_t MKFlags = 0;
76		25
77	const MMXLATION XLATIONS[] = {	26	const MMXLATION XLATIONS[] = {
78	{offsetof(ParamSet_t, levelCorrection[0]), offsetof(DynamicParams_t, levelCorrection[0]),0,255},	27	{offsetof(ParamSet_t, levelCorrection[0]), offsetof(DynamicParams_t, levelCorrection[0]),0,255},
79	{offsetof(ParamSet_t, levelCorrection[1]), offsetof(DynamicParams_t, levelCorrection[1]),0,255},	28	{offsetof(ParamSet_t, levelCorrection[1]), offsetof(DynamicParams_t, levelCorrection[1]),0,255},
80	{offsetof(ParamSet_t, gyroP), offsetof(DynamicParams_t, gyroP),0,255},	29	{offsetof(ParamSet_t, gyroP), offsetof(DynamicParams_t, gyroP),0,255},
81	{offsetof(ParamSet_t, gyroI), offsetof(DynamicParams_t, gyroI),0,255},	30	{offsetof(ParamSet_t, gyroI), offsetof(DynamicParams_t, gyroI),0,255},
82	{offsetof(ParamSet_t, gyroD), offsetof(DynamicParams_t, gyroD),0,255},	31	{offsetof(ParamSet_t, gyroD), offsetof(DynamicParams_t, gyroD),0,255},
83	{offsetof(ParamSet_t, attitudeControl), offsetof(DynamicParams_t, attitudeControl),0,255},	32	{offsetof(ParamSet_t, attitudeControl), offsetof(DynamicParams_t, attitudeControl),0,255},
84	{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255},	33	{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255},
85	{offsetof(ParamSet_t, dynamicStability), offsetof(DynamicParams_t, dynamicStability),0,255},	34	{offsetof(ParamSet_t, dynamicStability), offsetof(DynamicParams_t, dynamicStability),0,255},
86	{offsetof(ParamSet_t, heightP), offsetof(DynamicParams_t, heightP),0,255},	35	{offsetof(ParamSet_t, heightP), offsetof(DynamicParams_t, heightP),0,255},
87	{offsetof(ParamSet_t, heightI), offsetof(DynamicParams_t, heightI),0,255},	36	{offsetof(ParamSet_t, heightI), offsetof(DynamicParams_t, heightI),0,255},
88	{offsetof(ParamSet_t, heightD), offsetof(DynamicParams_t, heightD),0,255},	37	{offsetof(ParamSet_t, heightD), offsetof(DynamicParams_t, heightD),0,255},
89	{offsetof(ParamSet_t, heightSetting), offsetof(DynamicParams_t, heightSetting),0,255},	38	{offsetof(ParamSet_t, heightSetting), offsetof(DynamicParams_t, heightSetting),0,255},
90	{offsetof(ParamSet_t, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255},	39	{offsetof(ParamSet_t, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255},
91	{offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),0,255},	40	{offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),0,255},
92	{offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255},	41	{offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255},
93	{offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255},	42	{offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255},
94	{offsetof(ParamSet_t, naviMode), offsetof(DynamicParams_t, naviMode),0,255}};	43	{offsetof(ParamSet_t, naviMode), offsetof(DynamicParams_t, naviMode),0,255}};
95		44
96	uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {	45	uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {
97	uint8_t result;	46	uint8_t result;
98	if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)];	47	if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)];
99	else result = src;	48	else result = src;
100	if (result < min) result = min;	49	if (result < min) result = min;
101	else if (result > max) result = max;	50	else if (result > max) result = max;
102	return result;	51	return result;
103	}	52	}
104		53
105	void configuration_applyVariablesToParams(void) {	54	void configuration_applyVariablesToParams(void) {
106	uint8_t i, src;	55	uint8_t i, src;
107	uint8_t* pointerToTgt;	56	uint8_t* pointerToTgt;
108		57
109	for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) {	58	for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) {
110	src = ((uint8_t)(&staticParams) + XLATIONS[i].sourceIdx);	59	src = ((uint8_t)(&staticParams) + XLATIONS[i].sourceIdx);
111	pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx;	60	pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx;
112	*pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max);	61	*pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max);
113	}	62	}
114		63
115	// User parameters are always variable.	64	// User parameters are always variable.
116	for (i=0; i<sizeof(staticParams.userParams); i++) {	65	for (i=0; i<sizeof(staticParams.userParams); i++) {
117	src = ((uint8_t)(&staticParams) + offsetof(ParamSet_t, userParams) + i);	66	src = ((uint8_t)(&staticParams) + offsetof(ParamSet_t, userParams) + i);
118	pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i;	67	pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i;
119	*pointerToTgt = configuration_applyVariableToParam(src, 0, 255);	68	*pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
120	}	69	}
121	}	70	}
122		71
123	uint8_t getCPUType(void) { // works only after reset or power on when the registers have default values	-
124	uint8_t CPUType = ATMEGA644;	72	void setCPUType(void) { // works only after reset or power on when the registers have default values
125	if((UCSR1A == 0x20) && (UCSR1C == 0x06)) CPUType = ATMEGA644P; // initial Values for 644P after reset	73	if((UCSR1A == 0x20) && (UCSR1C == 0x06)) CPUType = ATMEGA644P; // initial Values for 644P after reset
126	return CPUType;	74	else CPUType = ATMEGA644;
127	}	75	}
128		76
129	/*	77	/*
130	* Automatic detection of hardware components is not supported in this development-oriented	78	* Automatic detection of hardware components is not supported in this development-oriented
131	* FC firmware. It would go against the point of it: To enable alternative hardware	79	* FC firmware. It would go against the point of it: To enable alternative hardware
132	* configurations with otherwise unsupported components. Instead, one should write	80	* configurations with otherwise unsupported components. Instead, one should write
133	* custom code + adjust constants for the new hardware, and include the relevant code	81	* custom code + adjust constants for the new hardware, and include the relevant code
134	* from the makefile.	82	* from the makefile.
135	* However - we still do detect the board release. Reason: Otherwise it would be too	83	* However - we still do detect the board release. Reason: Otherwise it would be too
136	* tedious to have to modify the code for how to turn on and off LEDs when deploying	84	* tedious to have to modify the code for how to turn on and off LEDs when deploying
137	* on different HW version....	85	* on different HW version....
138	*/	86	*/
139	uint8_t getBoardRelease(void) {	87	void setBoardRelease(void) {
140	uint8_t boardRelease = 13;	-
141	// the board release is coded via the pull up or down the 2 status LED	88	// the board release is coded via the pull up or down the 2 status LED
142		89
143	PORTB &= ~((1 << PORTB1)\|(1 << PORTB0)); // set tristate	90	PORTB &= ~((1 << PORTB1)\|(1 << PORTB0)); // set tristate
144	DDRB &= ~((1 << DDB0)\|(1 << DDB0)); // set port direction as input	91	DDRB &= ~((1 << DDB0)\|(1 << DDB0)); // set port direction as input
145		92
146	_delay_loop_2(1000); // make some delay	93	_delay_loop_2(1000); // make some delay
147		94
148	switch( PINB & ((1<<PINB1)\|(1<<PINB0))) {	95	switch( PINB & ((1<<PINB1)\|(1<<PINB0))) {
149	case 0x00:	96	case 0x00:
150	boardRelease = 10; // 1.0	97	boardRelease = 10; // 1.0
151	break;	98	break;
152	case 0x01:	99	case 0x01:
153	boardRelease = 11; // 1.1 or 1.2	100	boardRelease = 11; // 1.1 or 1.2
154	break;	101	break;
155	case 0x02:	102	case 0x02:
156	boardRelease = 20; // 2.0	103	boardRelease = 20; // 2.0
157	break;	104	break;
158	case 0x03:	105	case 0x03:
159	boardRelease = 13; // 1.3	106	boardRelease = 13; // 1.3
160	break;	107	break;
161	default:	108	default:
162	break;	109	break;
163	}	110	}
164	// set LED ports as output	111	// set LED ports as output
165	DDRB \|= (1<<DDB1)\|(1<<DDB0);	112	DDRB \|= (1<<DDB1)\|(1<<DDB0);
166	RED_OFF;	113	RED_OFF;
167	GRN_OFF;	114	GRN_OFF;
168	return boardRelease;	-
169	}	115	}
170		116
171	void configuration_setNormalFlightParameters(void) {	117	void configuration_setNormalFlightParameters(void) {
172	flight_setParameters(staticParams.IFactor, dynamicParams.gyroP,	118	flight_setParameters(staticParams.IFactor, dynamicParams.gyroP,
173	staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,	119	staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
174	dynamicParams.gyroP, staticParams.yawIFactor);	120	dynamicParams.gyroP, staticParams.yawIFactor);
175	}	121	}
176		122
177	void configuration_setFailsafeFlightParameters(void) {	123	void configuration_setFailsafeFlightParameters(void) {
178	flight_setParameters(0, 90, 120, 90, 120);	124	flight_setParameters(0, 90, 120, 90, 120);
179	}	125	}
180		126
181	// Called after a change in configuration parameters, as a hook for modules to take over changes.	127	// Called after a change in configuration parameters, as a hook for modules to take over changes.
182	void configuration_paramSetDidChange(void) {	128	void configuration_paramSetDidChange(void) {
183	// This should be OK to do here as long as we don't change parameters during emergency flight. We don't.	129	// This should be OK to do here as long as we don't change parameters during emergency flight. We don't.
184	configuration_setNormalFlightParameters();	130	configuration_setNormalFlightParameters();
185	// Immediately load changes to output, and also signal the paramset change.	131	// Immediately load changes to output, and also signal the paramset change.
186	output_init();	132	output_init();
187	}	133	}
188		134
189	void setOtherDefaults(void) {	135	void setOtherDefaults(void) {
190	// Height Control	136	// Height Control
191	staticParams.airpressureFilterConstant = 8;	137	staticParams.airpressureFilterConstant = 8;
192	//staticParams.airpressureWindowLength = 0;	138	//staticParams.airpressureWindowLength = 0;
193	staticParams.airpressureAccZCorrection = 128+56;	139	staticParams.airpressureAccZCorrection = 128+56;
194	staticParams.heightP = 10;	140	staticParams.heightP = 10;
195	staticParams.heightD = 30;	141	staticParams.heightD = 30;
196	staticParams.heightSetting = 251;	142	staticParams.heightSetting = 251;
197	staticParams.heightControlMaxThrottleChange = 10;	143	staticParams.heightControlMaxThrottleChange = 10;
198		144
199	// Control	145	// Control
200	staticParams.stickP = 8;	146	staticParams.stickP = 8;
201	staticParams.stickD = 12;	147	staticParams.stickD = 12;
202	staticParams.stickYawP = 12;	148	staticParams.stickYawP = 12;
203	staticParams.stickThrottleD = 12;	149	staticParams.stickThrottleD = 12;
204	staticParams.minThrottle = 8;	150	staticParams.minThrottle = 8;
205	staticParams.maxThrottle = 230;	151	staticParams.maxThrottle = 230;
206	staticParams.externalControl = 0;	152	staticParams.externalControl = 0;
207	staticParams.motorSmoothing = 0;	153	staticParams.motorSmoothing = 0;
208		154
209	// IMU	155	// IMU
210	staticParams.gyroPIDFilterConstant = 1;	156	staticParams.gyroPIDFilterConstant = 1;
211	staticParams.gyroATTFilterConstant = 1;	157	staticParams.gyroATTFilterConstant = 1;
212	staticParams.gyroDFilterConstant = 1;	158	staticParams.gyroDFilterConstant = 1;
213	staticParams.accFilterConstant = 10;	159	staticParams.accFilterConstant = 10;
214		160
215	staticParams.gyroP = 60;	161	staticParams.gyroP = 60;
216	staticParams.gyroI = 80;	162	staticParams.gyroI = 80;
217	staticParams.gyroD = 4;	163	staticParams.gyroD = 4;
218		164
219	// set by gyro-specific code: gyro_setDefaults().	165	// set by gyro-specific code: gyro_setDefaults().
220	// staticParams.zerothOrderCorrection =	166	// staticParams.zerothOrderCorrection =
221	// staticParams.driftCompDivider =	167	// staticParams.driftCompDivider =
222	// staticParams.driftCompLimit =	168	// staticParams.driftCompLimit =
223		169
224	staticParams.dynamicStability = 50;	170	staticParams.dynamicStability = 50;
225	staticParams.zerothOrderCorrectionAccTolerance = 60;	171	staticParams.zerothOrderCorrectionAccTolerance = 60;
226	staticParams.zerothOrderCorrectionControlTolerance = 60;	172	staticParams.zerothOrderCorrectionControlTolerance = 60;
227	staticParams.IFactor = 52;	173	staticParams.IFactor = 52;
228	staticParams.yawIFactor = 100;	174	staticParams.yawIFactor = 100;
229	staticParams.compassYawCorrection = 64;	175	staticParams.compassYawCorrection = 64;
230	staticParams.compassP = 50;	176	staticParams.compassP = 50;
231	staticParams.levelCorrection[0] = staticParams.levelCorrection[1] = 128;	177	staticParams.levelCorrection[0] = staticParams.levelCorrection[1] = 128;
232		178
233	// Servos	179	// Servos
234	staticParams.servoCount = 7;	180	staticParams.servoCount = 7;
235	staticParams.servoManualMaxSpeed = 10;	181	staticParams.servoManualMaxSpeed = 10;
236	for (uint8_t i=0; i<2; i++) {	182	for (uint8_t i=0; i<2; i++) {
237	staticParams.servoConfigurations[i].manualControl = 128;	183	staticParams.servoConfigurations[i].manualControl = 128;
238	staticParams.servoConfigurations[i].stabilizationFactor = 0;	184	staticParams.servoConfigurations[i].stabilizationFactor = 0;
239	staticParams.servoConfigurations[i].minValue = 32;	185	staticParams.servoConfigurations[i].minValue = 32;
240	staticParams.servoConfigurations[i].maxValue = 224;	186	staticParams.servoConfigurations[i].maxValue = 224;
241	staticParams.servoConfigurations[i].flags = 0;	187	staticParams.servoConfigurations[i].flags = 0;
242	}	188	}
243		189
244	// Battery warning and emergency flight	190	// Battery warning and emergency flight
245	staticParams.batteryVoltageWarning = 101; // 3.7 each for 3S	191	staticParams.batteryVoltageWarning = 101; // 3.7 each for 3S
246	staticParams.emergencyThrottle = 35;	192	staticParams.emergencyThrottle = 35;
247	staticParams.emergencyFlightDuration = 30;	193	staticParams.emergencyFlightDuration = 30;
248		194
249	// Outputs	195	// Outputs
250	staticParams.outputFlash[0].bitmask = 1; //0b01011111;	196	staticParams.outputFlash[0].bitmask = 1; //0b01011111;
251	staticParams.outputFlash[0].timing = 15;	197	staticParams.outputFlash[0].timing = 15;
252	staticParams.outputFlash[1].bitmask = 3; //0b11110011;	198	staticParams.outputFlash[1].bitmask = 3; //0b11110011;
253	staticParams.outputFlash[1].timing = 15;	199	staticParams.outputFlash[1].timing = 15;
254		200
255	staticParams.outputDebugMask = 8;	201	staticParams.outputDebugMask = 8;
256	staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP \| OUTPUTFLAGS_FLASH_1_AT_BEEP \| OUTPUTFLAGS_USE_ONBOARD_LEDS;	202	staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP \| OUTPUTFLAGS_FLASH_1_AT_BEEP \| OUTPUTFLAGS_USE_ONBOARD_LEDS;
257		203
258	staticParams.naviMode = 0; // free.	204	staticParams.naviMode = 0; // free.
259		205
260	staticParams.maxControlActivity = 0; // temporary!	206	staticParams.maxControlActivity = 0; // temporary!
261	staticParams.maxAccVector = 80;	207	staticParams.maxAccVector = 80;
262	}	208	}
263		209
264	/***************************************************/	210	/***************************************************/
265	/* Default Values for parameter set 1 */	211	/* Default Values for parameter set 1 */
266	/***************************************************/	212	/***************************************************/
267	void paramSet_default(uint8_t setnumber) {	213	void paramSet_default(uint8_t setnumber) {
268	setOtherDefaults();	214	setOtherDefaults();
269	gyro_setDefaultParameters();	215	gyro_setDefaultParameters();
270		216
271	for (uint8_t i=0; i<8; i++) {	217	for (uint8_t i=0; i<8; i++) {
272	staticParams.userParams[i] = 0;	218	staticParams.userParams[i] = 0;
273	}	219	}
274		220
275	staticParams.bitConfig =	221	staticParams.bitConfig =
276	CFG_GYRO_SATURATION_PREVENTION \| CFG_COMPASS_ENABLED;	222	CFG_GYRO_SATURATION_PREVENTION \| CFG_COMPASS_ENABLED;
277		223
278	memcpy(staticParams.name, "Default\0", 6);	224	memcpy(staticParams.name, "Default\0", 6);
279	}	225	}
280		226
281	/***************************************************/	227	/***************************************************/
282	/* Default Values for Mixer Table */	228	/* Default Values for Mixer Table */
283	/***************************************************/	229	/***************************************************/
284	void mixerMatrix_default(void) { // Quadro	230	void mixerMatrix_default(void) { // Quadro
285	uint8_t i;	231	uint8_t i;
286	// mixerMatric.revision = EEMIXER_REVISION;	232	// mixerMatric.revision = EEMIXER_REVISION;
287	// clear mixer table (but preset throttle)	233	// clear mixer table (but preset throttle)
288	for (i = 0; i < 16; i++) {	234	for (i = 0; i < 16; i++) {
289	mixerMatrix.motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0;	235	mixerMatrix.motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0;
290	mixerMatrix.motor[i][MIX_PITCH] = 0;	236	mixerMatrix.motor[i][MIX_PITCH] = 0;
291	mixerMatrix.motor[i][MIX_ROLL] = 0;	237	mixerMatrix.motor[i][MIX_ROLL] = 0;
292	mixerMatrix.motor[i][MIX_YAW] = 0;	238	mixerMatrix.motor[i][MIX_YAW] = 0;
293	}	239	}
294	// default = Quadro	240	// default = Quadro
295	mixerMatrix.motor[0][MIX_PITCH] = +64;	241	mixerMatrix.motor[0][MIX_PITCH] = +64;
296	mixerMatrix.motor[0][MIX_YAW] = +64;	242	mixerMatrix.motor[0][MIX_YAW] = +64;
297	mixerMatrix.motor[1][MIX_PITCH] = -64;	243	mixerMatrix.motor[1][MIX_PITCH] = -64;
298	mixerMatrix.motor[1][MIX_YAW] = +64;	244	mixerMatrix.motor[1][MIX_YAW] = +64;
299	mixerMatrix.motor[2][MIX_ROLL] = -64;	245	mixerMatrix.motor[2][MIX_ROLL] = -64;
300	mixerMatrix.motor[2][MIX_YAW] = -64;	246	mixerMatrix.motor[2][MIX_YAW] = -64;
301	mixerMatrix.motor[3][MIX_ROLL] = +64;	247	mixerMatrix.motor[3][MIX_ROLL] = +64;
302	mixerMatrix.motor[3][MIX_YAW] = -64;	248	mixerMatrix.motor[3][MIX_YAW] = -64;
303	memcpy(mixerMatrix.name, "Quadro\0", 7);	249	memcpy(mixerMatrix.name, "Quadro\0", 7);
304		250
305	/*	251	/*
306	// default = X	252	// default = X
307	mixerMatrix.motor[0][MIX_PITCH] = +45;	253	mixerMatrix.motor[0][MIX_PITCH] = +45;
308	mixerMatrix.motor[0][MIX_ROLL] = +45;	254	mixerMatrix.motor[0][MIX_ROLL] = +45;
309	mixerMatrix.motor[0][MIX_YAW] = +64;	255	mixerMatrix.motor[0][MIX_YAW] = +64;
310		256
311	mixerMatrix.motor[1][MIX_PITCH] = -45;	257	mixerMatrix.motor[1][MIX_PITCH] = -45;
312	mixerMatrix.motor[1][MIX_ROLL] = -45;	258	mixerMatrix.motor[1][MIX_ROLL] = -45;
313	mixerMatrix.motor[1][MIX_YAW] = +64;	259	mixerMatrix.motor[1][MIX_YAW] = +64;
314		260
315	mixerMatrix.motor[2][MIX_PITCH] = +45;	261	mixerMatrix.motor[2][MIX_PITCH] = +45;
316	mixerMatrix.motor[2][MIX_ROLL] = -45;	262	mixerMatrix.motor[2][MIX_ROLL] = -45;
317	mixerMatrix.motor[2][MIX_YAW] = -64;	263	mixerMatrix.motor[2][MIX_YAW] = -64;
318		264
319	mixerMatrix.motor[3][MIX_PITCH] = -45;	265	mixerMatrix.motor[3][MIX_PITCH] = -45;
320	mixerMatrix.motor[3][MIX_ROLL] = +45;	266	mixerMatrix.motor[3][MIX_ROLL] = +45;
321	mixerMatrix.motor[3][MIX_YAW] = -64;	267	mixerMatrix.motor[3][MIX_YAW] = -64;
322	*/	268	*/
323		269
324	memcpy(mixerMatrix.name, "X\0", 7);	270	memcpy(mixerMatrix.name, "X\0", 7);
325	}	271	}
326		272
327	/***************************************************/	273	/***************************************************/
328	/* Default Values for R/C Channels */	274	/* Default Values for R/C Channels */
329	/***************************************************/	275	/***************************************************/
330	void channelMap_default(void) {	276	void channelMap_default(void) {
331	channelMap.channels[CH_PITCH] = 1;	277	channelMap.channels[CH_PITCH] = 1;
332	channelMap.channels[CH_ROLL] = 0;	278	channelMap.channels[CH_ROLL] = 0;
333	channelMap.channels[CH_THROTTLE] = 2;	279	channelMap.channels[CH_THROTTLE] = 2;
334	channelMap.channels[CH_YAW] = 3;	280	channelMap.channels[CH_YAW] = 3;
335	channelMap.channels[CH_POTS + 0] = 4;	281	channelMap.channels[CH_POTS + 0] = 4;
336	channelMap.channels[CH_POTS + 1] = 5;	282	channelMap.channels[CH_POTS + 1] = 5;
337	channelMap.channels[CH_POTS + 2] = 6;	283	channelMap.channels[CH_POTS + 2] = 6;
338	channelMap.channels[CH_POTS + 3] = 7;	284	channelMap.channels[CH_POTS + 3] = 7;
339	}	285	}
340		286

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/configuration.c @ 2243 - Rev 2073 → 2088