WebSVN - FlightCtrl - Blame - Rev 1157 - /branches/thjac/V1

Rev	Author	Line No.	Line
1134	thjac	1	/* pitch.c
		2	*
		3	* Copyright 2009 Thomas Jachmann
		4	*
		5	* Pitch-Steuerung
		6	*/
		7
		8	#include "main.h"
		9	#include "parameter.h"
		10	#include "pitch_neutral.h"
		11	#include "pitch_md.h"
1157	krheinwald	12	#include "pitch_inc.h"
1134	thjac	13	#include "pitch.h"
		14
1141	thjac	15	#define UNDEFINED 255
		16	#define INVALID 255
1134	thjac	17
1141	thjac	18	int pitchInitialStickValue = UNDEFINED;
1139	thjac	19	unsigned char pitchMode;
		20
1141	thjac	21
1139	thjac	22	char *pitchModeStrings[] = {
1157	krheinwald	23	"Normal ",
		24	"Neutral ",
		25	"MD ",
		26	"Increment"
		27	"Undefined",
		28	"Undefined",
		29	"Undefined",
		30	"Invalid ",
1139	thjac	31	};
		32
		33
1134	thjac	34	// Zeiger auf den durch das Setting bestimmten Pitch-Steuerungsalgorithmus
1157	krheinwald	35	int (* pitchValueFP)( void );
1134	thjac	36
		37	// Prototyp
1157	krheinwald	38	void pitch_mk_init( void );
1134	thjac	39	int pitch_mk_value( void );
		40
		41	/*
		42	* F�hrt die Initialisierung der Pitch-Steuerung durch. Diese Funktion
		43	* wird nach jeder Setting-Auswahl sowie nach jeder Setting-�nderung
		44	* aufgerufen.
		45	*/
		46	void pitch_init( void ) {
		47
1157	krheinwald	48
		49	pitchMode = PARAM_PITCH_MODE;
		50	pitchValueFP = pitch_mk_value; // Reasonable default
		51
1141	thjac	52	// Nur beim Einschalten ermitteln, da beim Setting-Wechsel ung�ltig
		53	if( pitchInitialStickValue == UNDEFINED ) {
		54
		55	// Warten, bis ein g�ltiger Wert von der Fernsteuerung anliegt
		56	unsigned int timer = SetDelay( 500 );
		57	while( !CheckDelay( timer ) );
		58
		59	pitchInitialStickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ];
		60	}
		61
		62	/* Die Stick-Position beim Einschalten mu� mit dem im Setting eingestellten
		63	* Pitch-Modus �bereinstimmen. Sonst wird die Pitch-Steuerung aus Sicherheits-
		64	* gr�nden deaktiviert. Selbiges gilt, wenn die H�henregelung deaktiviert wurde.
		65	*/
1157	krheinwald	66
		67	switch( pitchMode ) {
1141	thjac	68	// Pitch-Modi mit neutralisiertem Gas
		69	case PARAM_PITCH_MODE_NEUTRAL:
1157	krheinwald	70	printf("\r\nPARAM_PITCH_MODE_NEUTRAL\r\n");
1141	thjac	71	case PARAM_PITCH_MODE_MD:
1157	krheinwald	72	printf("\r\nPARAM_PITCH_MODE_MD\r\n");
		73	if( !( EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG ) \|\| abs( pitchInitialStickValue ) > PARAM_PITCH_STICK_THRESHOLD )
1141	thjac	74	pitchMode = PARAM_PITCH_MODE_INVALID;
1157	krheinwald	75	else {
		76	if( pitchMode == PARAM_PITCH_MODE_NEUTRAL)
		77	pitch_neutral_init();
		78	else if( pitchMode == PARAM_PITCH_MODE_MD)
		79	pitch_md_init();
		80	}
		81	break;
		82
		83	// Pitch-Modi mit incrementellem Gas
		84	case PARAM_PITCH_MODE_INC:
		85	printf("\r\nPARAM_PITCH_MODE_INC\r\n");
1141	thjac	86	if( abs( pitchInitialStickValue ) > PARAM_PITCH_STICK_THRESHOLD )
		87	pitchMode = PARAM_PITCH_MODE_INVALID;
1157	krheinwald	88	else
		89	pitch_inc_init();
1141	thjac	90	break;
		91
		92	// Pitch-Modi mit 0-Gas
		93	case PARAM_PITCH_MODE_NORMAL:
1157	krheinwald	94	printf("\r\nPARAM_PITCH_MODE_NORMAL\r\n");
1141	thjac	95	if( pitchInitialStickValue > 35 - 120 )
		96	pitchMode = PARAM_PITCH_MODE_INVALID;
1157	krheinwald	97	else
		98	pitch_mk_init();
1141	thjac	99	break;
		100	}
1157	krheinwald	101
		102	if( pitchMode == PARAM_PITCH_MODE_INVALID)
		103	printf("\r\nPARAM_PITCH_MODE_INVALID, stick: %d\r\n", pitchInitialStickValue); //TODO
1141	thjac	104
1139	thjac	105
1157	krheinwald	106	/*
		107	// Moved to pitch_xxx_init()
		108	switch( pitchMode ) {
1134	thjac	109	case PARAM_PITCH_MODE_NEUTRAL:
1157	krheinwald	110	pitch_value_ptr = &pitch_neutral_value;
1134	thjac	111	break;
		112
		113	case PARAM_PITCH_MODE_MD:
1157	krheinwald	114	pitch_value_ptr = &pitch_md_value;
1134	thjac	115	break;
		116
1157	krheinwald	117	case PARAM_PITCH_MODE_INC:
		118	pitch_value_ptr = &pitch_inc_value;
		119	break;
		120
1134	thjac	121	default:
1157	krheinwald	122	pitch_value_ptr = &pitch_mk_value;
1134	thjac	123	}
1157	krheinwald	124	*/
1134	thjac	125
		126	// Hier k�nnen weitere Initialisierungen folgen
		127	}
		128
		129
1157	krheinwald	130	/*
		131	int pitch_value( void ) { // -> int (* pitch_value)( void );
		132
1139	thjac	133	switch( pitchMode ) {
1134	thjac	134	case PARAM_PITCH_MODE_NEUTRAL:
		135	return pitch_neutral_value();
		136
		137	case PARAM_PITCH_MODE_MD:
		138	return pitch_md_value();
		139
1141	thjac	140	case PARAM_PITCH_MODE_NORMAL:
		141	return pitch_mk_value();
		142
1157	krheinwald	143	case PARAM_PITCH_MODE_INC:
		144	return pitch_inc_value();
		145	break;
		146
1134	thjac	147	default:
1141	thjac	148	return 0;
1134	thjac	149	}
1157	krheinwald	150
		151
		152	return pitch_value_ptr();
		153
1134	thjac	154	}
1157	krheinwald	155	*/
1134	thjac	156
		157
		158	/*
		159	* F�hrt eine Pitch-Berechnung aus, die der Original-SW entspricht.
		160	*/
		161	int pitch_mk_value( void ) {
		162	register int stickValue = PPM_in[ EE_Parameter.Kanalbelegung[ K_GAS ] ];
1141	thjac	163	register int pitchCount = stickValue + 120;
1134	thjac	164
		165	DebugOut.Analog[26] = stickValue;
		166	DebugOut.Analog[28] = pitchCount;
		167
		168	return pitchCount;
		169	}
1157	krheinwald	170
		171	void pitch_mk_init( void ) {
		172
		173	printf("\r\npitch_mk_init()");
		174
		175	pitchValueFP = pitch_mk_value;
		176	}
		177
		178

Subversion Repositories FlightCtrl

(root)/branches/thjac/V1_11/pitch.c - Rev 1157