WebSVN - FlightCtrl - Diff - Rev 2141 and 2142 - /branches/dongfang_FC_fixedwing/arduino


#include <util/delay.h>
#include <stddef.h>
#include <string.h>
#include "configuration.h"
#include "controlMixer.h"
#include "rc.h"
#include "output.h"
#include "flight.h"
 
int16_t variables[VARIABLE_COUNT];
ParamSet_t staticParams;
ChannelMap_t channelMap;
RCTrim_t rcTrim;
IMUConfig_t IMUConfig;
volatile DynamicParams_t dynamicParams;
 
VersionInfo_t versionInfo;
 
FlightMode_t currentFlightMode = FLIGHT_MODE_MANUAL;
// updated by considering airspeed..
volatile uint16_t isFlying = 0;
 
const MMXLATION XLATIONS[] = {
{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255},
 
{offsetof(ParamSet_t, gyroPID[0].P), offsetof(DynamicParams_t, gyroPID[0].P),0,255},
{offsetof(ParamSet_t, gyroPID[0].I), offsetof(DynamicParams_t, gyroPID[0].I),0,255},
{offsetof(ParamSet_t, gyroPID[0].D), offsetof(DynamicParams_t, gyroPID[0].D),0,255},
 
{offsetof(ParamSet_t, gyroPID[1].P), offsetof(DynamicParams_t, gyroPID[1].P),0,255},
{offsetof(ParamSet_t, gyroPID[1].I), offsetof(DynamicParams_t, gyroPID[1].I),0,255},
{offsetof(ParamSet_t, gyroPID[1].D), offsetof(DynamicParams_t, gyroPID[1].D),0,255},
 
{offsetof(ParamSet_t, gyroPID[2].P), offsetof(DynamicParams_t, gyroPID[2].P),0,255},
{offsetof(ParamSet_t, gyroPID[2].I), offsetof(DynamicParams_t, gyroPID[2].I),0,255},
{offsetof(ParamSet_t, gyroPID[2].D), offsetof(DynamicParams_t, gyroPID[2].D),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},
};
 
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 configuration_setFlightParameters(uint8_t newFlightMode) {
        currentFlightMode = newFlightMode;
        flight_updateFlightParametersToFlightMode();
}
 
// 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(currentFlightMode);
  // Immediately load changes to output, and also signal the paramset change.
  output_init();
}
 
void setOtherDefaults(void) {
  // Control
  staticParams.externalControl = 0;
  staticParams.IFactor = 52;
 
  staticParams.airspeedCorrection = 1;
  staticParams.isFlyingThreshold = 10;
 
  // Servos
  staticParams.servoCount = 6;
  staticParams.servos[CONTROL_ELEVATOR].reverse = 0;
  staticParams.servos[CONTROL_AILERONS].reverse = 0; // 1 for extra.
  staticParams.servos[CONTROL_RUDDER].reverse = 0;
 
  for (uint8_t i=0; i<4; i++) {
      staticParams.servos[i].minValue = 80;
      staticParams.servos[i].maxValue = 80;
  }
 
  // Battery warning and emergency flight
  staticParams.batteryWarningVoltage = 101;  // 3.7 each for 3S
 
  for (uint8_t i=0; i<3; i++) {
          staticParams.gyroPID[i].P = 40;
          staticParams.gyroPID[i].I = 20;
          staticParams.gyroPID[i].D = 20;
          staticParams.gyroPID[i].iMax = 30; // 60 for extra.
  }
 
  staticParams.stickIElevator = 40;
  staticParams.stickIAilerons = 60;
  staticParams.stickIRudder = 20;
 
  // 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 = 0;
  staticParams.outputFlags   = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS;
}
 
/***************************************************/
/*    Default Values for parameter set 1           */
/***************************************************/
void paramSet_default(uint8_t setnumber) {
  setOtherDefaults();
 
  for (uint8_t i=0; i<8; i++) {
    staticParams.userParams[i] = i;
  }
 
  staticParams.bitConfig =
    CFG_GYRO_SATURATION_PREVENTION;
 
  memcpy(staticParams.name, "Default\0", 6);
}
 
void IMUConfig_default(void) {
  IMUConfig.gyroPIDFilterConstant = 10;
  IMUConfig.gyroDFilterConstant = 1;
  IMUConfig.rateTolerance = 120;
  IMUConfig.gyroDWindowLength = 8;
  IMUConfig.gyroQuadrant = 4;
  IMUConfig.imuReversedFlags = 0;
}
 
/***************************************************/
/*    Default Values for R/C Channels              */
/***************************************************/
void channelMap_default(void) {
  channelMap.RCPolarity = 1;
  channelMap.HWTrim = 175;
  channelMap.variableOffset = 131;
  channelMap.channels[CH_ELEVATOR] = 1;
  channelMap.channels[CH_AILERONS] = 0;
  channelMap.channels[CH_THROTTLE] = 2;
  channelMap.channels[CH_RUDDER]   = 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;
}
 

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.c - Rev 2141 → 2142

Rev 2141		Rev 2142
1	#include <util/delay.h>	1	#include <util/delay.h>
2	#include <stddef.h>	2	#include <stddef.h>
3	#include <string.h>	3	#include <string.h>
4	#include "configuration.h"	4	#include "configuration.h"
5	#include "controlMixer.h"	5	#include "controlMixer.h"
6	#include "rc.h"	6	#include "rc.h"
7	#include "output.h"	7	#include "output.h"
8	#include "flight.h"	8	#include "flight.h"
9		9
10	int16_t variables[VARIABLE_COUNT];	10	int16_t variables[VARIABLE_COUNT];
11	ParamSet_t staticParams;	11	ParamSet_t staticParams;
12	ChannelMap_t channelMap;	12	ChannelMap_t channelMap;
13	RCTrim_t rcTrim;	13	RCTrim_t rcTrim;
14	IMUConfig_t IMUConfig;	14	IMUConfig_t IMUConfig;
15	volatile DynamicParams_t dynamicParams;	15	volatile DynamicParams_t dynamicParams;
16		16
17	VersionInfo_t versionInfo;	17	VersionInfo_t versionInfo;
18		18
19	FlightMode_t currentFlightMode = FLIGHT_MODE_MANUAL;	19	FlightMode_t currentFlightMode = FLIGHT_MODE_MANUAL;
20	// updated by considering airspeed..	20	// updated by considering airspeed..
21	volatile uint16_t isFlying = 0;	21	volatile uint16_t isFlying = 0;
22		22
23	const MMXLATION XLATIONS[] = {	23	const MMXLATION XLATIONS[] = {
24	{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255},	24	{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255},
25		25
26	{offsetof(ParamSet_t, gyroPID[0].P), offsetof(DynamicParams_t, gyroPID[0].P),0,255},	26	{offsetof(ParamSet_t, gyroPID[0].P), offsetof(DynamicParams_t, gyroPID[0].P),0,255},
27	{offsetof(ParamSet_t, gyroPID[0].I), offsetof(DynamicParams_t, gyroPID[0].I),0,255},	27	{offsetof(ParamSet_t, gyroPID[0].I), offsetof(DynamicParams_t, gyroPID[0].I),0,255},
28	{offsetof(ParamSet_t, gyroPID[0].D), offsetof(DynamicParams_t, gyroPID[0].D),0,255},	28	{offsetof(ParamSet_t, gyroPID[0].D), offsetof(DynamicParams_t, gyroPID[0].D),0,255},
29		29
30	{offsetof(ParamSet_t, gyroPID[1].P), offsetof(DynamicParams_t, gyroPID[1].P),0,255},	30	{offsetof(ParamSet_t, gyroPID[1].P), offsetof(DynamicParams_t, gyroPID[1].P),0,255},
31	{offsetof(ParamSet_t, gyroPID[1].I), offsetof(DynamicParams_t, gyroPID[1].I),0,255},	31	{offsetof(ParamSet_t, gyroPID[1].I), offsetof(DynamicParams_t, gyroPID[1].I),0,255},
32	{offsetof(ParamSet_t, gyroPID[1].D), offsetof(DynamicParams_t, gyroPID[1].D),0,255},	32	{offsetof(ParamSet_t, gyroPID[1].D), offsetof(DynamicParams_t, gyroPID[1].D),0,255},
33		33
34	{offsetof(ParamSet_t, gyroPID[2].P), offsetof(DynamicParams_t, gyroPID[2].P),0,255},	34	{offsetof(ParamSet_t, gyroPID[2].P), offsetof(DynamicParams_t, gyroPID[2].P),0,255},
35	{offsetof(ParamSet_t, gyroPID[2].I), offsetof(DynamicParams_t, gyroPID[2].I),0,255},	35	{offsetof(ParamSet_t, gyroPID[2].I), offsetof(DynamicParams_t, gyroPID[2].I),0,255},
36	{offsetof(ParamSet_t, gyroPID[2].D), offsetof(DynamicParams_t, gyroPID[2].D),0,255},	36	{offsetof(ParamSet_t, gyroPID[2].D), offsetof(DynamicParams_t, gyroPID[2].D),0,255},
37		37
38	{offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255},	38	{offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255},
39	{offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255},	39	{offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255},
40	};	40	};
41		41
42	uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {	42	uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) {
43	uint8_t result;	43	uint8_t result;
44	if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)];	44	if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)];
45	else result = src;	45	else result = src;
46	if (result < min) result = min;	46	if (result < min) result = min;
47	else if (result > max) result = max;	47	else if (result > max) result = max;
48	return result;	48	return result;
49	}	49	}
50		50
51	void configuration_applyVariablesToParams(void) {	51	void configuration_applyVariablesToParams(void) {
52	uint8_t i, src;	52	uint8_t i, src;
53	uint8_t* pointerToTgt;	53	uint8_t* pointerToTgt;
54		54
55	for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) {	55	for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) {
56	src = ((uint8_t)(&staticParams) + XLATIONS[i].sourceIdx);	56	src = ((uint8_t)(&staticParams) + XLATIONS[i].sourceIdx);
57	pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx;	57	pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx;
58	*pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max);	58	*pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max);
59	}	59	}
60		60
61	// User parameters are always variable.	61	// User parameters are always variable.
62	for (i=0; i<sizeof(staticParams.userParams); i++) {	62	for (i=0; i<sizeof(staticParams.userParams); i++) {
63	src = ((uint8_t)(&staticParams) + offsetof(ParamSet_t, userParams) + i);	63	src = ((uint8_t)(&staticParams) + offsetof(ParamSet_t, userParams) + i);
64	pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i;	64	pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i;
65	*pointerToTgt = configuration_applyVariableToParam(src, 0, 255);	65	*pointerToTgt = configuration_applyVariableToParam(src, 0, 255);
66	}	66	}
67	}	67	}
68		68
69	void configuration_setFlightParameters(uint8_t newFlightMode) {	69	void configuration_setFlightParameters(uint8_t newFlightMode) {
70	currentFlightMode = newFlightMode;	70	currentFlightMode = newFlightMode;
71	flight_updateFlightParametersToFlightMode();	71	flight_updateFlightParametersToFlightMode();
72	}	72	}
73		73
74	// Called after a change in configuration parameters, as a hook for modules to take over changes.	74	// Called after a change in configuration parameters, as a hook for modules to take over changes.
75	void configuration_paramSetDidChange(void) {	75	void configuration_paramSetDidChange(void) {
76	// This should be OK to do here as long as we don't change parameters during emergency flight. We don't.	76	// This should be OK to do here as long as we don't change parameters during emergency flight. We don't.
77	configuration_setFlightParameters(currentFlightMode);	77	configuration_setFlightParameters(currentFlightMode);
78	// Immediately load changes to output, and also signal the paramset change.	78	// Immediately load changes to output, and also signal the paramset change.
79	output_init();	79	output_init();
80	}	80	}
81		81
82	void setOtherDefaults(void) {	82	void setOtherDefaults(void) {
83	// Control	83	// Control
84	staticParams.externalControl = 0;	84	staticParams.externalControl = 0;
85	staticParams.IFactor = 52;	85	staticParams.IFactor = 52;
86		86
87	staticParams.airspeedCorrection = 1;	87	staticParams.airspeedCorrection = 1;
88	staticParams.isFlyingThreshold = 100;	88	staticParams.isFlyingThreshold = 10;
89		89
90	// Servos	90	// Servos
91	staticParams.servoCount = 7;	91	staticParams.servoCount = 6;
92	staticParams.servos[CONTROL_ELEVATOR].reverse = 0;	92	staticParams.servos[CONTROL_ELEVATOR].reverse = 0;
93	staticParams.servos[CONTROL_AILERONS].reverse = 1;	93	staticParams.servos[CONTROL_AILERONS].reverse = 0; // 1 for extra.
-		94	staticParams.servos[CONTROL_RUDDER].reverse = 0;
-		95
-		96	for (uint8_t i=0; i<4; i++) {
-		97	staticParams.servos[i].minValue = 80;
-		98	staticParams.servos[i].maxValue = 80;
94	staticParams.servos[CONTROL_RUDDER].reverse = 0;	99	}
95		100
96	// Battery warning and emergency flight	101	// Battery warning and emergency flight
97	staticParams.batteryWarningVoltage = 101; // 3.7 each for 3S	102	staticParams.batteryWarningVoltage = 101; // 3.7 each for 3S
98		103
99	for (uint8_t i=0; i<3; i++) {	104	for (uint8_t i=0; i<3; i++) {
100	staticParams.gyroPID[i].P = 40;	105	staticParams.gyroPID[i].P = 40;
101	staticParams.gyroPID[i].I = 40;	106	staticParams.gyroPID[i].I = 20;
102	staticParams.gyroPID[i].D = 40;	107	staticParams.gyroPID[i].D = 20;
103	staticParams.gyroPID[i].iMax = 60;	108	staticParams.gyroPID[i].iMax = 30; // 60 for extra.
104	}	109	}
105		110
106	staticParams.stickIElevator = 40;	111	staticParams.stickIElevator = 40;
107	staticParams.stickIAilerons = 60;	112	staticParams.stickIAilerons = 60;
108	staticParams.stickIRudder = 20;	113	staticParams.stickIRudder = 20;
109		114
110	// Outputs	115	// Outputs
111	staticParams.outputFlash[0].bitmask = 1; //0b01011111;	116	staticParams.outputFlash[0].bitmask = 1; //0b01011111;
112	staticParams.outputFlash[0].timing = 15;	117	staticParams.outputFlash[0].timing = 15;
113	staticParams.outputFlash[1].bitmask = 3; //0b11110011;	118	staticParams.outputFlash[1].bitmask = 3; //0b11110011;
114	staticParams.outputFlash[1].timing = 15;	119	staticParams.outputFlash[1].timing = 15;
115		120
116	staticParams.outputDebugMask = 8;	121	staticParams.outputDebugMask = 0;
117	staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP \| OUTPUTFLAGS_FLASH_1_AT_BEEP \| OUTPUTFLAGS_USE_ONBOARD_LEDS;	122	staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP \| OUTPUTFLAGS_FLASH_1_AT_BEEP \| OUTPUTFLAGS_USE_ONBOARD_LEDS;
118	}	123	}
119		124
120	/***************************************************/	125	/***************************************************/
121	/* Default Values for parameter set 1 */	126	/* Default Values for parameter set 1 */
122	/***************************************************/	127	/***************************************************/
123	void paramSet_default(uint8_t setnumber) {	128	void paramSet_default(uint8_t setnumber) {
124	setOtherDefaults();	129	setOtherDefaults();
125		130
126	for (uint8_t i=0; i<8; i++) {	131	for (uint8_t i=0; i<8; i++) {
127	staticParams.userParams[i] = i;	132	staticParams.userParams[i] = i;
128	}	133	}
129		134
130	staticParams.bitConfig =	135	staticParams.bitConfig =
131	CFG_GYRO_SATURATION_PREVENTION;	136	CFG_GYRO_SATURATION_PREVENTION;
132		137
133	memcpy(staticParams.name, "Default\0", 6);	138	memcpy(staticParams.name, "Default\0", 6);
134	}	139	}
135		140
136	void IMUConfig_default(void) {	141	void IMUConfig_default(void) {
137	IMUConfig.gyroPIDFilterConstant = 10;	142	IMUConfig.gyroPIDFilterConstant = 10;
138	IMUConfig.gyroDFilterConstant = 1;	143	IMUConfig.gyroDFilterConstant = 1;
139	IMUConfig.rateTolerance = 120;	144	IMUConfig.rateTolerance = 120;
140	IMUConfig.gyroDWindowLength = 8;	145	IMUConfig.gyroDWindowLength = 8;
141	IMUConfig.gyroQuadrant = 4;	146	IMUConfig.gyroQuadrant = 4;
142	IMUConfig.imuReversedFlags = IMU_REVERSE_GYRO_PR;	147	IMUConfig.imuReversedFlags = 0;
143	}	148	}
144		149
145	/***************************************************/	150	/***************************************************/
146	/* Default Values for R/C Channels */	151	/* Default Values for R/C Channels */
147	/***************************************************/	152	/***************************************************/
148	void channelMap_default(void) {	153	void channelMap_default(void) {
149	channelMap.RCPolarity = 1;	154	channelMap.RCPolarity = 1;
150	channelMap.HWTrim = 175;	155	channelMap.HWTrim = 175;
151	channelMap.variableOffset = 131;	156	channelMap.variableOffset = 131;
152	channelMap.channels[CH_ELEVATOR] = 1;	157	channelMap.channels[CH_ELEVATOR] = 1;
153	channelMap.channels[CH_AILERONS] = 0;	158	channelMap.channels[CH_AILERONS] = 0;
154	channelMap.channels[CH_THROTTLE] = 2;	159	channelMap.channels[CH_THROTTLE] = 2;
155	channelMap.channels[CH_RUDDER] = 3;	160	channelMap.channels[CH_RUDDER] = 3;
156	channelMap.channels[CH_POTS + 0] = 4;	161	channelMap.channels[CH_POTS + 0] = 4;
157	channelMap.channels[CH_POTS + 1] = 5;	162	channelMap.channels[CH_POTS + 1] = 5;
158	channelMap.channels[CH_POTS + 2] = 6;	163	channelMap.channels[CH_POTS + 2] = 6;
159	channelMap.channels[CH_POTS + 3] = 7;	164	channelMap.channels[CH_POTS + 3] = 7;
160	}	165	}
161		166