ov_mm3.ks

////////////////////////////////////////////////////////////////////////////////
//	OV		MAJOR MODE 3 (LANDING)
////////////////////////////////////////////////////////////////////////////////
FUNCTION MM3_ENTER {
	// VELOCITY MONITOR
	GLOBAL PREVIOUS_POSITION TO V(0,0,0).
	
	// CALCULATE GLIDESLOPE PARAMETERS
	GLOBAL H0 TO MM3_FLARE_ALT + MM3_H1_PARAM.
	GLOBAL D1 TO MM3_FLARE_ALT/TAN(MM3_GLIDE_ANG).
	GLOBAL FLARE_CONST TO (H0*D1)/MM3_FLARE_ALT.
	GLOBAL FLARE_LENGTH TO -LN(MM3_H1_PARAM/H0)*FLARE_CONST.
	GLOBAL FLARE_DISPLACEMENT TO FLARE_LENGTH - D1.
	
	// HEADING SELECTION (TO PERMIT RLTS)
	GLOBAL HEADING_SIGN TO 1.
	IF GEOPOSITION:LNG > RUNWAY:LNG {
		GLOBAL HEADING_SIGN TO -1.
	}
	
	// INITIALIZE CONTROLLERS
	GLOBAL APPROACH_CONTROLLER TO	PIDLOOP(0.028,	0.000,	0.028,  -9.0, 12.5).
	GLOBAL GS_CONTROLLER TO			PIDLOOP(0.200,	0.050,	0.350,  -9.0, 15.5).
	GLOBAL FLARE_CONTROLLER TO		PIDLOOP(1.000,	0.000,	0.600,	-2.0, 7.5).
	GLOBAL TARGET_ENERGY TO 10000000.
	
	// INITIALIZE COMMANDS
	SET PITCH_CMD TO 0.
	SET HEADING_CMD TO 90.
	SET CURRENT_STEERING TO HEADING(90,0).
	
	// INITIALIZE GUIDANCE VARIABLES
	GLOBAL HNAV_DELTA TO 0.
	GLOBAL VNAV_DELTA TO 0.
	GLOBAL HNAV TO 0.
	GLOBAL VNAV TO 0.
	GLOBAL SLOPE TO 0.
	GLOBAL TOTAL_ENERGY TO 0.
	GLOBAL NAV_ALT TO 0.
	GLOBAL VX TO 0.
	GLOBAL VY TO 0.
	GLOBAL VZ TO 0.
	
	// DOWNMODE/UPMODE IF NEEDED
	IF AIRSPEED < 1 {
		SET MINOR_MODE TO 5.
		SET RESTART TO FALSE.
	}
}

FUNCTION MM3_LEAVE {
	DISABLE_STEERING().
	BRAKES OFF.
}

FUNCTION MM3_TRANSFER {
	IF (MINOR_MODE = 0) OR (MINOR_MODE >= 3) {
		DISABLE_STEERING().
		BRAKES OFF.
	} ELSE {
		ENABLE_STEERING().
	}
	
	// ALWAYS SCHEDULE TASK, UNLESS STANDBY
	IF MINOR_MODE < 5 {
		TASK_SCHEDULE(1, MM3_GUIDANCE_TASK@).
	}
}

FUNCTION MM3_COMMAND {
	PARAMETER VERB, VALUE.
	
	// NOTHING
}

FUNCTION MM3_GUIDANCE_TASK { PARAMETER DT.
	IF DT = INIT { // HACK
		RETURN.
	}
	
	//
	// RCS CONTROL LOOP
	//
	IF ((SHIP:Q < 0.05) AND (MINOR_MODE <= 2)) {
		RCS ON.
	} ELSE {
		RCS OFF.
	}

	// HORIZONTAL NAVIGATION
	LOCAL HNAV_MOD IS MAX(0, MIN(1, (RUNWAY:DISTANCE - 20000)/25000 )).
	SET HNAV_DELTA TO (1000*(GEOPOSITION:LAT - RUNWAY:LAT)).
	SET HNAV TO MIN(MAX( HNAV_DELTA*(0.25 - 0.20*HNAV_MOD),-25),25).

	// VERTICAL NAVIGATION
	SET NAV_ALT TO SHIP:ALTITUDE - RUNWAY_ASL.
	IF RUNWAY:DISTANCE > FLARE_LENGTH { // ON GLIDE SLOPE
		SET SLOPE TO (RUNWAY:DISTANCE - FLARE_DISPLACEMENT)*TAN(MM3_GLIDE_ANG).
	} ELSE { // ON FLARE
		LOCAL EXP_PARAM IS -MAX(0, FLARE_LENGTH - RUNWAY:DISTANCE)/FLARE_CONST.
		LOCAL EXP_VALUE IS CONSTANT:E^EXP_PARAM.
		SET SLOPE TO (H0*EXP_VALUE - MM3_H1_PARAM).
	}
	
	// GLIDE SLOPE TOWARDS 75% OF FINAL ALTITUDE (SO FINAL GLIDE STARTS FOR SURE)
	SET VNAV_DELTA TO (NAV_ALT - (MM3_FINAL_ALT*0.75 + SLOPE)).
	IF MINOR_MODE >= 2 { // DESCENT CONTROL
		SET VNAV TO GS_CONTROLLER:UPDATE(TIME:SECONDS, VNAV_DELTA).
	} ELSE { // APPROACH CONTROL
		SET VNAV TO GS_CONTROLLER:UPDATE(TIME:SECONDS, VNAV_DELTA).
	}
	
	//
	// ENERGY MANAGEMENT. FIXME: THIS MUST BE CONFIGURABLE BETTER
	//
	SET TOTAL_ENERGY TO (AIRSPEED^2)/2 + 9.81*NAV_ALT.
	// ALTITUDE ENERGY GRADIENT
	LOCAL ALT_ENERGY IS MAX(0,MIN(1,RUNWAY:DISTANCE/20000)).
	// VELOCITY ENERGY GRADIENT
	LOCAL V_ENERGY IS MAX(0,MIN(1,(RUNWAY:DISTANCE-20000)/30000)).
	// ENERGY SETPOINT
	LOCAL SET_ENERGY IS ((MM3_TARGET_SPEED + 650*V_ENERGY)^2)/2 + 75000*MAX(0,MIN(1,RUNWAY:DISTANCE/20000)).
	SET TARGET_ENERGY TO MIN(TARGET_ENERGY, SET_ENERGY).
	// ONLY MANAGE DYNAMIC PRESSURE
	IF RUNWAY:DISTANCE > 35000 {
		SET TARGET_ENERGY TO 100000000.
	}
	
	// COMPUTE RELATIVE SPEEDS
	SET VX TO (RUNWAY:DISTANCE - PREVIOUS_POSITION:X)/DT.
	SET VY TO (HNAV_DELTA - PREVIOUS_POSITION:Y)/DT.
	SET VZ TO (NAV_ALT - PREVIOUS_POSITION:Z)/DT.
	SET PREVIOUS_POSITION:X TO RUNWAY:DISTANCE.
	SET PREVIOUS_POSITION:Y TO HNAV_DELTA.
	SET PREVIOUS_POSITION:Z TO NAV_ALT.

	// FINAL FLARE CONTROL OVERRIDE
	IF ALT:RADAR < MM3_FINAL_ALT {
		// BLEND BETWEEN 5 M/S AND 1 M/S VERTICAL VELOCITY
		LOCAL VNAV_MOD IS MAX(0, MIN(1, (ALT:RADAR-MM3_GLIDE_ALT)/(MM3_FINAL_ALT*0.5) )).
		// FLARE CORRECTION
		LOCAL VNAV_CORR IS FLARE_CONTROLLER:UPDATE(TIME:SECONDS, VZ+ (1+4*VNAV_MOD) ).
		
		// GRADIENT FROM GLIDE SLOPE TO FINAL FLARE
		SET VNAV_DELTA TO (MM3_FINAL_ALT - ALT:RADAR)/20.
		SET VNAV TO MAX(VNAV, 17.0*MIN(MAX(VNAV_DELTA,0),1) + VNAV_CORR).
	}
	
	// MODE TRANSITIONS
	IF RUNWAY:DISTANCE < 25000 { // TRANSFER TO MODE 32 FOR DESCENT
		IF MINOR_MODE = 1 {
			TRANSFER_MODE(3,2).
		}
	}
	IF ALT:RADAR < 6 { // TRANSFER TO MODE 33 ON TOUCHDOWN
		IF MINOR_MODE = 2 {
			TRANSFER_MODE(3,3).
		}
	}
	IF MODE_TIMER() > 3.0 { // TRANSFER TO MODE 34 AFTER STABILIZING
		IF MINOR_MODE = 3 {
			STAGE. // OPEN CHUTE: FIXME
			TRANSFER_MODE(3,4).
		}
	}
	IF MINOR_MODE = 4 { // TRANSFER TO MODE 35 AFTER A FULL STOP
		IF AIRSPEED < 1 {
			TRANSFER_MODE(3,5).
		}
	}


	// DESCENT/APPROACH
	IF (MINOR_MODE = 1) OR (MINOR_MODE = 2) {
		IF (TOTAL_ENERGY > TARGET_ENERGY) OR (SHIP:Q > 0.18) {
			BRAKES ON.
		}
		IF (TOTAL_ENERGY < TARGET_ENERGY) AND (SHIP:Q < 0.18) {
			BRAKES OFF.
		}
		IF ALT:RADAR < MM3_FINAL_ALT { // FINAL FLARE
			BRAKES OFF.
		}
		
		LOCAL TGT_HEADING_CMD IS 90 + HNAV.
		LOCAL TGT_PITCH_CMD IS -10 + VNAV.
		SET PITCH_CMD TO TGT_PITCH_CMD.
		
		//
		// RATE LIMIT THE HEADING COMMAND
		//
		IF HEADING_CMD < TGT_HEADING_CMD {
			SET HEADING_CMD TO MIN(TGT_HEADING_CMD, HEADING_CMD + MM3_HEADING_RATE*DT).
		}
		IF HEADING_CMD > TGT_HEADING_CMD {
			SET HEADING_CMD TO MAX(TGT_HEADING_CMD, HEADING_CMD - MM3_HEADING_RATE*DT).
		}
		
		//
		// RATE LIMIT THE PITCH COMMAND
		//
		//IF PITCH_CMD < TGT_PITCH_CMD {
		//	SET PITCH_CMD TO MIN(TGT_PITCH_CMD, PITCH_CMD + MM3_PITCH_RATE*DT).
		//}
		//IF PITCH_CMD > TGT_PITCH_CMD {
		//	SET PITCH_CMD TO MAX(TGT_PITCH_CMD, PITCH_CMD - MM3_PITCH_RATE*DT).
		//}
	
		// STEERING LOOP
		SET CURRENT_STEERING TO HEADING(HEADING_SIGN*HEADING_CMD, PITCH_CMD).
		
		// FORCE SAS OFF
		SAS OFF.
	}
	// LANDING
	IF MINOR_MODE = 3 {
		BRAKES OFF.
		DISABLE_STEERING().
	}
	// BRAKE
	IF MINOR_MODE = 4 {
		//CHUTES ON. // FIXME DOESNT WORK
		BRAKES ON.
		DISABLE_STEERING().
	}
	
	// LOWER GEAR
	IF ALTITUDE < 325 {
		GEAR ON.
	}
	
	// SCHEDULE TASK UNLESS IN STANDBY MODE
	IF MINOR_MODE < 5 {
		TASK_SCHEDULE(1, MM3_GUIDANCE_TASK@).
	}
}

FUNCTION MM3_UI_TASK { PARAMETER DT.
	UI_VARIABLE("RADALT",	"M",	ALT:RADAR, 								0,7, NUMBER,	0,0).
	UI_VARIABLE("NAVALT",	"M",	NAV_ALT, 								0,7, NUMBER,	0,1).
	UI_VARIABLE("E",		"KJ",	(TOTAL_ENERGY - TARGET_ENERGY)/1000,	2,7, SIGNED,	0,2).
	UI_VARIABLE("Q",		"KPA",	SHIP:Q*100, 							2,5, NUMBER,	18,2).
	
	UI_VARIABLE("NAV +X",	"M",	10*ROUND(RUNWAY:DISTANCE/10,0), 		0,7, SIGNED,	0,4).
	UI_VARIABLE("NAV +Y",	"M",	HNAV_DELTA, 							3,7, SIGNED,	0,5).
	UI_VARIABLE("NAV +Z",	"M",	VNAV_DELTA, 							3,7, SIGNED,	0,6).
	
	UI_VARIABLE("   +VX",	"M",	AIRSPEED, 								1,7, SIGNED,	0,8).
	UI_VARIABLE("   +VY",	"M",	VY, 									1,7, SIGNED,	0,9).
	UI_VARIABLE("   +VZ",	"M",	VZ, 									1,7, SIGNED,	0,10).

	UI_VARIABLE("BRAKE",	"",		BRAKES, 								0,4, ONOFF,		0,12).
	UI_VARIABLE("GEAR",		"",		GEAR, 									0,4, ONOFF,		16,12).
	
	TASK_SCHEDULE(4, MM3_UI_TASK@).
}


FUNCTION MM3_TELEMETRY_TASK { PARAMETER DT.
	DOWNLINK("ALT     ",ROUND(ALT:RADAR,2) ).
	DOWNLINK("TOT E",	ROUND(TOTAL_ENERGY,0) ).
	DOWNLINK("TGT E",	ROUND(TARGET_ENERGY,0) ).
	DOWNLINK("HNAV D",	ROUND(HNAV_DELTA,3) ).
	DOWNLINK("VNAV D",	ROUND(VNAV_DELTA,3) ).
	DOWNLINK("Q",		ROUND(SHIP:Q*100,3) ).
	DOWNLINK("VY",		ROUND(VY,2) ).
	DOWNLINK("VZ",		ROUND(VZ,2) ).
	IF BRAKES {
		DOWNLINK("BRAKE", 1).
	} ELSE {
		DOWNLINK("BRAKE", 0).
	}
	IF GEAR {
		DOWNLINK("GEAR", 1).
	} ELSE {
		DOWNLINK("GEAR", 0).
	}

	TASK_SCHEDULE(7, MM3_TELEMETRY_TASK@).
}.




////////////////////////////////////////////////////////////////////////////////
MODE_NAMES:ADD(30, "FLIGHT MONITOR  ").
MODE_NAMES:ADD(31, "APPROACH PHASE  ").
MODE_NAMES:ADD(32, "DESCENT PHASE   ").
MODE_NAMES:ADD(33, "TOUCHDOWN PHASE ").
MODE_NAMES:ADD(34, "BRAKING PHASE   ").
MODE_NAMES:ADD(35, "STANDBY LANDED  ").

MODE_ENTER		(3, MM3_ENTER@).
MODE_TRANSFER	(3, MM3_TRANSFER@).
MODE_LEAVE		(3, MM3_LEAVE@).
MODE_COMMAND	(3, MM3_COMMAND@).

MODE_TASK		(3, MM3_GUIDANCE_TASK@).
MODE_TASK		(3, MM3_UI_TASK@).
MODE_TASK		(3, MM3_TELEMETRY_TASK@).