(DEFINE-CIRCUIT ANGLE-LPF (:INPUTS CLOCK ANGLE TIME-CONST) (:OUTPUTS)) (DEFINE-CIRCUIT RECT-TO-POLAR (:INPUTS X Y) (:OUTPUTS R THETA) (:COMPONENT SQRT-0 R (SQRT W-6235)) (:COMPONENT +-0 W-6235 (+ W-6233 W-6234)) (:COMPONENT SQUARE-1 W-6234 (SQUARE Y)) (:COMPONENT SQUARE-0 W-6233 (SQUARE X)) (:COMPONENT ATAN-0 THETA (ATAN Y X))) (DEFINE-CIRCUIT POLAR-TO-RECT (:INPUTS R THETA) (:OUTPUTS X Y) (:COMPONENT *-1 Y (* R W-19)) (:COMPONENT SIN-0 W-19 (SIN THETA)) (:COMPONENT *-0 X (* R W-18)) (:COMPONENT COS-0 W-18 (COS THETA))) (DEFINE-CIRCUIT SMP_FDIR (:INPUTS CLOCK D-HEAD D-VEL D-ALT) (:OUTPUTS D-HEAD VEL ALT) (:REGISTER-INIT TIME-CONST 20) (:COMPONENT LPF-2 ALT (LPF CLOCK D-ALT TIME-CONST)) (:COMPONENT LPF-1 VEL (LPF CLOCK D-VEL TIME-CONST))) (DEFINE-CIRCUIT SMP_GUID (:INPUTS CLOCK CPOS) (:OUTPUTS D-HEAD DES-VEL DES-CLIMB) (:REGISTER-INIT LEAD-TIME *LEAD-TIME*) (:COMPONENT RECT-TO-POLAR-1 W-16 D-HEAD (RECT-TO-POLAR W-6 W-3)) (:COMPONENT --0 W-3 (- W-1 W-2)) (:COMPONENT 3-VECTOR-Y-1 W-2 (3-VECTOR-Y CPOS)) (:COMPONENT --1 W-6 (- W-4 W-5)) (:COMPONENT 3-VECTOR-X-1 W-5 (3-VECTOR-X CPOS)) (:COMPONENT /-0 DES-VEL (/ W-16 LEAD-TIME)) (:COMPONENT /-2 DES-CLIMB (/ W-21 LEAD-TIME)) (:COMPONENT --5 W-21 (- DES-ALT W-20)) (:COMPONENT 3-VECTOR-Z-0 DES-ALT (3-VECTOR-Z DPOS)) (:COMPONENT 3-VECTOR-Y-0 W-1 (3-VECTOR-Y DPOS)) (:COMPONENT 3-VECTOR-X-0 W-4 (3-VECTOR-X DPOS)) (:COMPONENT FLIGHT-PLAN-1 PCPOS DPOS (FLIGHT-PLAN CLOCK)) (:COMPONENT 3-VECTOR-Z-2 W-20 (3-VECTOR-Z CPOS))) (DEFINE-CIRCUIT WASHOUT_FILTER_INAV_DNS (:INPUTS CLOCK MODE1 MODE2 MODE3) (:OUTPUTS MODE1)) (DEFINE-CIRCUIT GPS_INS_NAV_MODE (:INPUTS CLOCK SENSOR-DATA) (:OUTPUTS DATA) (:COMPONENT MAKE-NAV-DATA-0 DATA (MAKE-NAV-DATA POS VEL)) (:COMPONENT 3-VECTOR-COMPLEMENTARY-FILTER-0 POS (3-VECTOR-COMPLEMENTARY-FILTER CLOCK W-770 W-771)) (:COMPONENT SENSOR-DATA-INS-POS-0 W-770 (SENSOR-DATA-INS-POS SENSOR-DATA)) (:COMPONENT SENSOR-DATA-GPS-POS-0 W-771 (SENSOR-DATA-GPS-POS SENSOR-DATA)) (:COMPONENT SENSOR-DATA-INS-VEL-0 VEL (SENSOR-DATA-INS-VEL SENSOR-DATA))) (DEFINE-CIRCUIT GPS_NAV_MODE (:INPUTS CLOCK SENSOR-DATA) (:OUTPUTS NAV-DATA) (:COMPONENT MAKE-NAV-DATA-0 NAV-DATA (MAKE-NAV-DATA GPS-POS GPS-VEL)) (:COMPONENT 3-VECTOR-DERIVATIVE-0 GPS-VEL (3-VECTOR-DERIVATIVE CLOCK GPS-POS W-769)) (:COMPONENT DYNAMIC-DELTA-0 W-769 (DYNAMIC-DELTA)) (:COMPONENT SENSOR-DATA-GPS-POS-0 GPS-POS (SENSOR-DATA-GPS-POS SENSOR-DATA))) (DEFINE-CIRCUIT SMP_NAV (:INPUTS CLOCK SENSOR-DATA) (:OUTPUTS POS) (:COMPONENT NAV-DATA-POS-0 POS (NAV-DATA-POS NAV_DATA)) (:COMPONENT WASHOUT_FILTER_INAV_DNS-2 NAV_DATA (WASHOUT_FILTER_INAV_DNS CLOCK GPS-INS-MODE-DATA GPS-MODE-DATA INS-MODE-DATA)) (:COMPONENT GPS_INS_NAV_MODE-0 GPS-INS-MODE-DATA (GPS_INS_NAV_MODE CLOCK SENSOR-DATA)) (:COMPONENT GPS_NAV_MODE-0 GPS-MODE-DATA (GPS_NAV_MODE CLOCK SENSOR-DATA)) (:COMPONENT INS_NAV_MODE-0 INS-MODE-DATA (INS_NAV_MODE CLOCK SENSOR-DATA))) (DEFINE-CIRCUIT INS_NAV_MODE (:INPUTS CLOCK SENSOR-DATA) (:OUTPUTS NAV-DATA) (:COMPONENT MAKE-NAV-DATA-0 NAV-DATA (MAKE-NAV-DATA W-767 W-768)) (:COMPONENT SENSOR-DATA-INS-VEL-0 W-768 (SENSOR-DATA-INS-VEL SENSOR-DATA)) (:COMPONENT SENSOR-DATA-INS-POS-0 W-767 (SENSOR-DATA-INS-POS SENSOR-DATA))) (DEFINE-CIRCUIT ONE (:INPUTS) (:OUTPUTS ONE) (:REGISTER-INIT ONE 1.0)) (DEFINE-CIRCUIT INVERSE (:INPUTS X) (:OUTPUTS Y) (:COMPONENT /-0 Y (/ W-595 X)) (:COMPONENT ONE-0 W-595 (ONE))) (DEFINE-CIRCUIT 3-VECTOR-DERIVATIVE (:INPUTS CLOCK VECTOR DELTA) (:OUTPUTS DERIV) (:REGISTER-INIT PREV-VECTOR 3-VECTOR-0) (:COMPONENT 3D-SCALAR-MULT-0 DERIV (3D-SCALAR-MULT W-593 W-594)) (:COMPONENT 3D-DIFFERENCE-0 W-594 (3D-DIFFERENCE VECTOR PREV-VECTOR)) (:COMPONENT INVERSE-0 W-593 (INVERSE DELTA)) (:REGISTER-INPUT PREV-VECTOR VECTOR CLOCK)) (DEFINE-CIRCUIT 3-VECTOR-INTEGRAL (:INPUTS CLOCK VECTOR DELTA) (:OUTPUTS INT-VECTOR) (:REGISTER-INIT INT-VECTOR 3D-ZERO) (:COMPONENT 3D-SUM-1 NEXT-VECTOR (3D-SUM INT-VECTOR W-503)) (:COMPONENT 3D-SCALAR-MULT-0 W-503 (3D-SCALAR-MULT DELTA VECTOR)) (:REGISTER-INPUT INT-VECTOR NEXT-VECTOR CLOCK)) (DEFINE-CIRCUIT INS_ASN_141 (:INPUTS CLOCK VEL) (:OUTPUTS INS-POS NVEL) (:REGISTER-INIT NMAG 0.01) (:COMPONENT 3D-SUM-0 INS-POS (3D-SUM W-501 W-502)) (:COMPONENT 3-VECTOR-INTEGRAL-0 W-501 (3-VECTOR-INTEGRAL CLOCK NVEL DELTA)) (:COMPONENT DYNAMIC-DELTA-0 DELTA (DYNAMIC-DELTA)) (:COMPONENT INITIAL-POS-0 W-502 (INITIAL-POS)) (:COMPONENT ADD-3-VECTOR-NOISE-0 NVEL (ADD-3-VECTOR-NOISE CLOCK VEL NMAG))) (DEFINE-CIRCUIT DYNAMIC-DELTA (:INPUTS) (:OUTPUTS DELTA) (:REGISTER-INIT DELTA 0.05)) (DEFINE-CIRCUIT INITIAL-POS (:INPUTS) (:OUTPUTS INIT-POS) (:REGISTER-INIT INIT-Z 1000) (:REGISTER-INIT INIT-Y 0) (:REGISTER-INIT INIT-X 0) (:COMPONENT MAKE-3-VECTOR-0 INIT-POS (MAKE-3-VECTOR INIT-X INIT-Y INIT-Z))) (DEFINE-CIRCUIT ADD-3-VECTOR-NOISE (:INPUTS CLOCK VECTOR NMAG) (:OUTPUTS NVECTOR) (:COMPONENT 3D-SUM-0 NVECTOR (3D-SUM NOISE VECTOR)) (:COMPONENT MAKE-3-VECTOR-0 NOISE (MAKE-3-VECTOR W-492 W-496 W-500)) (:COMPONENT *-5 W-500 (* NMAG W-499)) (:COMPONENT *-4 W-499 (* W-497 W-498)) (:COMPONENT NOISE-2 W-498 (NOISE CLOCK)) (:COMPONENT 3-VECTOR-Z-0 W-497 (3-VECTOR-Z VECTOR)) (:COMPONENT *-3 W-496 (* NMAG W-495)) (:COMPONENT *-2 W-495 (* W-493 W-494)) (:COMPONENT NOISE-1 W-494 (NOISE CLOCK)) (:COMPONENT 3-VECTOR-Y-0 W-493 (3-VECTOR-Y VECTOR)) (:COMPONENT *-1 W-492 (* W-490 W-491)) (:COMPONENT NOISE-0 W-491 (NOISE CLOCK)) (:COMPONENT *-0 W-490 (* NMAG W-489)) (:COMPONENT 3-VECTOR-X-0 W-489 (3-VECTOR-X VECTOR))) (DEFINE-CIRCUIT POS-NOISE (:INPUTS CLOCK) (:OUTPUTS)) (DEFINE-CIRCUIT GPS_ARN_149V2 (:INPUTS CLOCK POS) (:OUTPUTS MPOS) (:REGISTER-INIT NMAG 0.01) (:COMPONENT ADD-3-VECTOR-NOISE-0 MPOS (ADD-3-VECTOR-NOISE CLOCK POS NMAG))) (DEFINE-CIRCUIT SENSOR-DATA (:INPUTS CLOCK POS VEL) (:OUTPUTS SENSOR-DATA) (:COMPONENT MAKE-SENSOR-DATA-0 SENSOR-DATA (MAKE-SENSOR-DATA GPS-POS INS-POS INS-VEL)) (:COMPONENT INS_ASN_141-1 INS-POS INS-VEL (INS_ASN_141 CLOCK VEL)) (:COMPONENT GPS_ARN_149V2-0 GPS-POS (GPS_ARN_149V2 CLOCK POS))) (DEFINE-CIRCUIT SMP_PMODEL (:INPUTS CLOCK HEAD VEL CLIMB) (:OUTPUTS POS ACT-VEL) (:REGISTER-INIT MAX-VEL 3.0) (:COMPONENT MIN-0 ADJ-VEL (MIN MAX-VEL VEL)) (:COMPONENT POLAR-TO-RECT-0 XDOT YDOT (POLAR-TO-RECT ADJ-VEL HEAD)) (:COMPONENT 3-VECTOR-DERIVATIVE-0 ACT-VEL (3-VECTOR-DERIVATIVE CLOCK POS DELTA)) (:COMPONENT 3D-SUM-0 POS (3D-SUM POS-VECTOR W-17)) (:COMPONENT 3-VECTOR-INTEGRAL-0 POS-VECTOR (3-VECTOR-INTEGRAL CLOCK VEL-VECTOR DELTA)) (:COMPONENT DYNAMIC-DELTA-0 DELTA (DYNAMIC-DELTA)) (:COMPONENT MAKE-3-VECTOR-1 VEL-VECTOR (MAKE-3-VECTOR XDOT YDOT CLIMB)) (:COMPONENT INITIAL-POS-0 W-17 (INITIAL-POS))) (DEFINE-CIRCUIT LPF (:INPUTS CLOCK SIG TIME-CONST) (:OUTPUTS AV-SIG) (:REGISTER-INIT ONE 1) (:REGISTER-INIT AV-SIG 0) (:COMPONENT +-0 NEW-AV (+ W-333 W-334)) (:COMPONENT *-0 W-333 (* W1 SIG)) (:COMPONENT *-1 W-334 (* W2 AV-SIG)) (:COMPONENT --0 W2 (- ONE W1)) (:COMPONENT /-0 W1 (/ ONE TIME-CONST)) (:REGISTER-INPUT AV-SIG NEW-AV CLOCK)) (DEFINE-CIRCUIT SMP_AVIONICS (:INPUTS C-CLOCK D-CLOCK) (:OUTPUTS) (:COMPONENT SMP_FDIR-0 HEAD VEL ALT (SMP_FDIR D-CLOCK D-HEAD D-VEL D-ALT)) (:COMPONENT SMP_GUID-0 D-HEAD D-VEL D-ALT (SMP_GUID C-CLOCK NAV-DATA)) (:COMPONENT SMP_NAV-0 NAV-DATA (SMP_NAV D-CLOCK SENSOR-DATA)) (:COMPONENT SMP_PMODEL-2 ACT-POS ACT-VEL (SMP_PMODEL D-CLOCK HEAD VEL ALT)) (:COMPONENT SENSOR-DATA-0 SENSOR-DATA (SENSOR-DATA D-CLOCK ACT-POS ACT-VEL))) (DEFINE-CIRCUIT SMP_GUID (:INPUTS CLOCK CPOS) (:OUTPUTS D-HEAD DES-VEL DES-CLIMB) (:REGISTER-INIT LEAD-TIME *LEAD-TIME*) (:COMPONENT RECT-TO-POLAR-1 W-16 D-HEAD (RECT-TO-POLAR W-6 W-3)) (:COMPONENT --0 W-3 (- W-1 W-2)) (:COMPONENT 3-VECTOR-Y-1 W-2 (3-VECTOR-Y CPOS)) (:COMPONENT --1 W-6 (- W-4 W-5)) (:COMPONENT 3-VECTOR-X-1 W-5 (3-VECTOR-X CPOS)) (:COMPONENT /-0 DES-VEL (/ W-16 LEAD-TIME)) (:COMPONENT /-2 DES-CLIMB (/ W-21 LEAD-TIME)) (:COMPONENT --5 W-21 (- DES-ALT W-20)) (:COMPONENT 3-VECTOR-Z-0 DES-ALT (3-VECTOR-Z DPOS)) (:COMPONENT 3-VECTOR-Y-0 W-1 (3-VECTOR-Y DPOS)) (:COMPONENT 3-VECTOR-X-0 W-4 (3-VECTOR-X DPOS)) (:COMPONENT FLIGHT-PLAN-1 PCPOS DPOS (FLIGHT-PLAN CLOCK)) (:COMPONENT 3-VECTOR-Z-2 W-20 (3-VECTOR-Z CPOS))) (DEFINE-CIRCUIT GUID (:INPUTS CLOCK CPOS) (:OUTPUTS DES-HD DES-VEL DES-ALT) (:REGISTER-INIT LEAD-TIME *LEAD-TIME*) (:COMPONENT ATAN-0 DES-HD (ATAN W-3 W-6)) (:COMPONENT --0 W-3 (- W-1 W-2)) (:COMPONENT 3-VECTOR-Y-1 W-2 (3-VECTOR-Y CPOS)) (:COMPONENT --1 W-6 (- W-4 W-5)) (:COMPONENT 3-VECTOR-X-1 W-5 (3-VECTOR-X CPOS)) (:COMPONENT /-0 DES-VEL (/ W-16 LEAD-TIME)) (:COMPONENT SQRT-0 W-16 (SQRT W-15)) (:COMPONENT +-0 W-15 (+ W-10 W-14)) (:COMPONENT SQUARE-0 W-10 (SQUARE W-9)) (:COMPONENT --2 W-9 (- W-7 W-8)) (:COMPONENT 3-VECTOR-X-2 W-7 (3-VECTOR-X CPOS)) (:COMPONENT SQUARE-1 W-14 (SQUARE W-13)) (:COMPONENT --3 W-13 (- W-11 W-12)) (:COMPONENT 3-VECTOR-Y-2 W-11 (3-VECTOR-Y CPOS)) (:COMPONENT 3-VECTOR-Z-0 DES-ALT (3-VECTOR-Z DPOS)) (:COMPONENT 3-VECTOR-Y-0 W-1 (3-VECTOR-Y DPOS)) (:COMPONENT 3-VECTOR-X-0 W-4 (3-VECTOR-X DPOS)) (:COMPONENT 3-VECTOR-X-3 W-8 (3-VECTOR-X DPOS)) (:COMPONENT 3-VECTOR-Y-3 W-12 (3-VECTOR-Y DPOS)) (:COMPONENT FLIGHT-PLAN-1 PCPOS DPOS (FLIGHT-PLAN CLOCK))) (DEFINE-CIRCUIT SMP_PMODEL (:INPUTS CLOCK HEAD VEL CLIMB) (:OUTPUTS POS ACT-VEL) (:REGISTER-INIT MAX-VEL 3.0) (:COMPONENT MIN-0 ADJ-VEL (MIN MAX-VEL VEL)) (:COMPONENT POLAR-TO-RECT-0 XDOT YDOT (POLAR-TO-RECT ADJ-VEL HEAD)) (:COMPONENT 3-VECTOR-DERIVATIVE-0 ACT-VEL (3-VECTOR-DERIVATIVE CLOCK POS DELTA)) (:COMPONENT 3D-SUM-0 POS (3D-SUM POS-VECTOR W-17)) (:COMPONENT 3-VECTOR-INTEGRAL-0 POS-VECTOR (3-VECTOR-INTEGRAL CLOCK VEL-VECTOR DELTA)) (:COMPONENT DYNAMIC-DELTA-0 DELTA (DYNAMIC-DELTA)) (:COMPONENT MAKE-3-VECTOR-1 VEL-VECTOR (MAKE-3-VECTOR XDOT YDOT CLIMB)) (:COMPONENT INITIAL-POS-0 W-17 (INITIAL-POS))) (DEFINE-CIRCUIT SMP_AVIONICS (:INPUTS C-CLOCK D-CLOCK) (:OUTPUTS) (:COMPONENT SMP_FDIR-0 HEAD VEL ALT (SMP_FDIR D-CLOCK D-HEAD D-VEL D-ALT)) (:COMPONENT SMP_GUID-0 D-HEAD D-VEL D-ALT (SMP_GUID C-CLOCK NAV-DATA)) (:COMPONENT SMP_NAV-0 NAV-DATA (SMP_NAV D-CLOCK SENSOR-DATA)) (:COMPONENT SMP_PMODEL-2 ACT-POS ACT-VEL (SMP_PMODEL D-CLOCK HEAD VEL ALT)) (:COMPONENT SENSOR-DATA-0 SENSOR-DATA (SENSOR-DATA D-CLOCK ACT-POS ACT-VEL)))