# set moddebug 2
module load graph
@graph timespan 30
@alias add minscore remote camera set minscore
@alias add minscore2 remote camera set minscore2
@alias add movejoe wp movemulti 22 15 27
@alias add joedropdist wp param 22 2
@alias add wpedit module load misseditor
@alias add terminate param set AFS_TERMINATE
@alias add termaction param set AFS_TERM_ACTION
@alias add drop servo set 8 1500
@alias add hold servo set 8 900
@alias add bdrop servo set 8 1500
@alias add bhold servo set 8 1000
@alias add drop2 servo set 6 1050
@alias add hold2 servo set 6 1480
@alias add odroidpower relay set 0
@alias add neutral2 servo set 12 1500
@alias add ekf param set AHRS_EKF_USE
@alias add gpsdisable param set SIM_GPS_DISABLE
@alias add bb status gps* scaled* raw*
@alias add g graph
@alias add grc3 g RC_CHANNELS.chan3_raw SERVO_OUTPUT_RAW.servo3_raw
@alias add grc25 g RC_CHANNELS.chan2_raw SERVO_OUTPUT_RAW.servo2_raw SERVO_OUTPUT_RAW.servo5_raw
@alias add gflaperon g SERVO_OUTPUT_RAW.servo1_raw SERVO_OUTPUT_RAW.servo7_raw SERVO_OUTPUT_RAW.servo8_raw
@alias add gtracker g wrap_360(degrees(ATTITUDE.yaw)) NAV_CONTROLLER_OUTPUT.target_bearing degrees(ATTITUDE.pitch) NAV_CONTROLLER_OUTPUT.nav_pitch
@alias add gtrackerror g angle_diff(degrees(ATTITUDE.yaw),NAV_CONTROLLER_OUTPUT.target_bearing) angle_diff(degrees(ATTITUDE.pitch),NAV_CONTROLLER_OUTPUT.nav_pitch)
@alias add gaccel g RAW_IMU.xacc*9.81*0.001 RAW_IMU.yacc*9.81*0.001 RAW_IMU.zacc*9.81*0.001 gravity(RAW_IMU)
@alias add gaccelcmp g RAW_IMU.xacc*9.81*0.001 RAW_IMU.yacc*9.81*0.001 RAW_IMU.zacc*9.81*0.001 SCALED_IMU2.xacc*9.81*0.001 SCALED_IMU2.yacc*9.81*0.001 SCALED_IMU2.zacc*9.81*0.001
@alias add gaccelcmp3 g RAW_IMU.xacc*9.81*0.001 RAW_IMU.yacc*9.81*0.001 RAW_IMU.zacc*9.81*0.001 SCALED_IMU2.xacc*9.81*0.001 SCALED_IMU2.yacc*9.81*0.001 SCALED_IMU2.zacc*9.81*0.001 SCALED_IMU3.xacc*9.81*0.001 SCALED_IMU3.yacc*9.81*0.001 SCALED_IMU3.zacc*9.81*0.001
@alias add gaccel2 g SCALED_IMU2.xacc*9.81*0.001 SCALED_IMU2.yacc*9.81*0.001 SCALED_IMU2.zacc*9.81*0.001 gravity(SCALED_IMU2)
@alias add gaccel3 g SCALED_IMU3.xacc*9.81*0.001 SCALED_IMU3.yacc*9.81*0.001 SCALED_IMU3.zacc*9.81*0.001 gravity(SCALED_IMU3)
@alias add gaccelxy g RAW_IMU.xacc*9.81*0.001 RAW_IMU.yacc*9.81*0.001
@alias add gaccelz_filter g RAW_IMU.zacc*9.81*0.001 lowpass(RAW_IMU.zacc*9.81*0.001,"z",0.9)
@alias add ggyro  g degrees(RAW_IMU.xgyro*0.001) degrees(RAW_IMU.ygyro*0.001) degrees(RAW_IMU.zgyro*0.001)
@alias add ggyrocmp g degrees(RAW_IMU.xgyro*0.001) degrees(RAW_IMU.ygyro*0.001) degrees(RAW_IMU.zgyro*0.001) degrees(SCALED_IMU2.xgyro*0.001) degrees(SCALED_IMU2.ygyro*0.001) degrees(SCALED_IMU2.zgyro*0.001)
@alias add ggyro2  g degrees(SCALED_IMU2.xgyro*0.001) degrees(SCALED_IMU2.ygyro*0.001) degrees(SCALED_IMU2.zgyro*0.001)
@alias add ggyro_earth  g earth_gyro(RAW_IMU,ATTITUDE).x earth_gyro(RAW_IMU,ATTITUDE).y earth_gyro(RAW_IMU,ATTITUDE).z
@alias add grp g degrees(ATTITUDE.roll) degrees(ATTITUDE.pitch)
@alias add grpsim g degrees(ATTITUDE.roll) degrees(ATTITUDE.pitch) degrees(SIMSTATE.roll) degrees(SIMSTATE.pitch)
@alias add ignitioncut rc 7 1000
@alias add ignitionon rc 7 2000
@alias add gsim g degrees(ATTITUDE.roll) degrees(ATTITUDE.pitch) degrees(ATTITUDE.yaw) degrees(SIMSTATE.roll) degrees(SIMSTATE.pitch) degrees(SIMSTATE.yaw)
@alias add gsimahrs2 g degrees(AHRS2.roll) degrees(AHRS2.pitch) degrees(AHRS2.yaw) degrees(SIMSTATE.roll) degrees(SIMSTATE.pitch) degrees(SIMSTATE.yaw)
@alias add gahrs2 g degrees(AHRS2.roll) degrees(AHRS2.pitch) degrees(AHRS2.yaw) degrees(ATTITUDE.roll) degrees(ATTITUDE.pitch) degrees(ATTITUDE.yaw)
@alias add gahrs2rp g degrees(AHRS2.roll) degrees(AHRS2.pitch) degrees(ATTITUDE.roll) degrees(ATTITUDE.pitch)
@alias add gahrs3 g degrees(AHRS3.roll) degrees(AHRS3.pitch) degrees(AHRS3.yaw) degrees(ATTITUDE.roll) degrees(ATTITUDE.pitch) degrees(ATTITUDE.yaw)
@alias add gahrs2rp g degrees(AHRS2.roll) degrees(AHRS2.pitch) degrees(ATTITUDE.roll) degrees(ATTITUDE.pitch)
@alias add gahrsanuroll g degrees(ATTITUDE.roll) degrees(AHRS2.roll) degrees(AHRS3.roll)
@alias add gahrsanupitch g degrees(ATTITUDE.pitch) degrees(AHRS2.pitch) degrees(AHRS3.pitch)
@alias add gcmppitch g degrees(SIMSTATE.pitch) degrees(ATTITUDE.pitch) degrees(AHRS2.pitch)
@alias add gcmproll g degrees(SIMSTATE.roll) degrees(ATTITUDE.roll) degrees(AHRS2.roll)
@alias add grollp g NAV_CONTROLLER_OUTPUT.nav_roll-degrees(ATTITUDE.roll) (SERVO_OUTPUT_RAW.servo1_raw-MAV.params['RC1_TRIM'])*0.1 
@alias add gradio g RADIO.rssi RADIO.noise RADIO.remrssi RADIO.remnoise 
@alias add gbatt g SYS_STATUS.current_battery
@alias add gnavroll g NAV_CONTROLLER_OUTPUT.nav_roll-degrees(ATTITUDE.roll) (SERVO_OUTPUT_RAW.servo1_raw-MAV.params['RC1_TRIM'])*0.1
@alias add gcontrol g RC_CHANNELS.chan1_raw RC_CHANNELS.chan2_raw SERVO_OUTPUT_RAW.servo1_raw SERVO_OUTPUT_RAW.servo2_raw
@alias add gmag g RAW_IMU.xmag RAW_IMU.ymag RAW_IMU.zmag mag_field(RAW_IMU)
@alias add gmagcmp g RAW_IMU.xmag RAW_IMU.ymag RAW_IMU.zmag SCALED_IMU2.xmag SCALED_IMU2.ymag SCALED_IMU2.zmag
@alias add gmagcmp3 g RAW_IMU.xmag RAW_IMU.ymag RAW_IMU.zmag SCALED_IMU2.xmag SCALED_IMU2.ymag SCALED_IMU2.zmag SCALED_IMU3.xmag SCALED_IMU3.ymag SCALED_IMU3.zmag
@alias add gmag2 g SCALED_IMU2.xmag SCALED_IMU2.ymag SCALED_IMU2.zmag mag_field(SCALED_IMU2)
@alias add gmag3 g SCALED_IMU3.xmag SCALED_IMU3.ymag SCALED_IMU3.zmag mag_field(SCALED_IMU3)
@alias add gmagfield g mag_field(RAW_IMU)
@alias add gmagofs g SENSOR_OFFSETS.mag_ofs_x SENSOR_OFFSETS.mag_ofs_y SENSOR_OFFSETS.mag_ofs_z
@alias add gservo g SERVO_OUTPUT_RAW.servo1_raw SERVO_OUTPUT_RAW.servo2_raw SERVO_OUTPUT_RAW.servo3_raw SERVO_OUTPUT_RAW.servo4_raw
@alias add gservo6 g SERVO_OUTPUT_RAW.servo1_raw SERVO_OUTPUT_RAW.servo2_raw SERVO_OUTPUT_RAW.servo3_raw SERVO_OUTPUT_RAW.servo4_raw SERVO_OUTPUT_RAW.servo5_raw SERVO_OUTPUT_RAW.servo6_raw
@alias add gservo6 g SERVO_OUTPUT_RAW.servo1_raw SERVO_OUTPUT_RAW.servo2_raw SERVO_OUTPUT_RAW.servo3_raw SERVO_OUTPUT_RAW.servo4_raw SERVO_OUTPUT_RAW.servo5_raw SERVO_OUTPUT_RAW.servo6_raw SERVO_OUTPUT_RAW.servo7_raw SERVO_OUTPUT_RAW.servo8_raw
@alias add gservo12 g SERVO_OUTPUT_RAW.servo1_raw SERVO_OUTPUT_RAW.servo2_raw
@alias add gthr g VFR_HUD.throttle
@alias add ggs g VFR_HUD.groundspeed
@alias add grc g RC_CHANNELS.chan1_raw RC_CHANNELS.chan2_raw RC_CHANNELS.chan3_raw RC_CHANNELS.chan4_raw RC_CHANNELS.chan5_raw RC_CHANNELS.chan6_raw RC_CHANNELS.chan7_raw RC_CHANNELS.chan8_raw
@alias add grc4 g RC_CHANNELS.chan1_raw RC_CHANNELS.chan2_raw RC_CHANNELS.chan3_raw RC_CHANNELS.chan4_raw
@alias add grc12 g RC_CHANNELS.chan1_raw RC_CHANNELS.chan2_raw
@alias add galt g GLOBAL_POSITION_INT.relative_alt*0.001 altitude(SCALED_PRESSURE)
@alias add gpress g SCALED_PRESSURE.press_abs
@alias add gclimb g lowpass(delta(altitude(SCALED_PRESSURE),"a"),"a2",0.9) VFR_HUD.climb GLOBAL_POSITION_INT.vz*0.01
@alias add gvz g GLOBAL_POSITION_INT.vz*0.01
@alias add ghead g VFR_HUD.heading mag_heading(RAW_IMU,ATTITUDE)
@alias add gnavrp g NAV_CONTROLLER_OUTPUT.nav_roll NAV_CONTROLLER_OUTPUT.nav_pitch degrees(ATTITUDE.roll) degrees(ATTITUDE.pitch)
@alias add gspeed g VFR_HUD.groundspeed 
@alias add gthr g VFR_HUD.throttle
@alias add galterr g NAV_CONTROLLER_OUTPUT.alt_error
@alias add gtargetalt g GLOBAL_POSITION_INT.relative_alt*0.001+NAV_CONTROLLER_OUTPUT.alt_error
@alias add gthrpitch g VFR_HUD.throttle NAV_CONTROLLER_OUTPUT.nav_pitch
@alias add gsteer g NAV_CONTROLLER_OUTPUT.nav_roll NAV_CONTROLLER_OUTPUT.nav_pitch
@alias add gspeed g VFR_HUD.airspeed VFR_HUD.groundspeed
@alias add ggpst g min(abs(diff(GPS_RAW_INT.time_usec*1.0e-6,"d")),3)
@alias add gload g SYS_STATUS.load
@alias add grate g 1000.0/diff(ATTITUDE.time_boot_ms,"a")
@alias add ggpsrate g 1.0e6/diff(GPS_RAW_INT.time_usec,"g")
@alias add grange g rangefinder_roll(RANGEFINDER,ATTITUDE,maxdist=39.5) GLOBAL_POSITION_INT.relative_alt*0.001
@alias add grangealt g RANGEFINDER.distance GLOBAL_POSITION_INT.relative_alt*0.001
@alias add grangev g RANGEFINDER.voltage
@alias add gahrserr g AHRS.error_rp AHRS.error_yaw
@alias add gage g GPS2_RAW.dgps_age
@alias add gomega g degrees(AHRS.omegaIx) degrees(AHRS.omegaIy) degrees(AHRS.omegaIz)
# jonos farm
#
#[9:38 PM] (Channel) Jono: These are very rough estimates, I'm not entirely sure how far north the boundry extends, but the south side is relatively true
#SE -34.671287° 150.599052°
#SW -34.669365° 150.590907°
#
#[9:38 PM] (Channel) Jono: aaand north
#[9:39 PM] (Channel) Jono: NW -34.666365° 150.594070°
#NE -34.667936° 150.602835°
#
#map icon -34.668400 150.597192

# Geoff Kemp farm
#map icon -34.946217 149.201569

# ANU Spring Farm Valley
# map icon -35.2869,149.0097

@alias add uwa map icon -31.979968 115.817737
@alias add hil10 map icon -31.82552913 115.73857757

@alias add d1 wp show /home/tridge/project/UAV/WP/UWA-test2.txt
@alias add d2 map icon -31.979968 115.817737
@alias add d3 map icon -31.82552913 115.73857757

# joe pos at CMAC
@alias add joe1 map icon -35.362779 149.164248 barrell
@alias add joe2 map icon -35.363131 149.164166 barrell
@alias add gtakeoff graph VFR_HUD.airspeed NAV_CONTROLLER_OUTPUT.nav_pitch NAV_CONTROLLER_OUTPUT.alt_error NAV_CONTROLLER_OUTPUT.aspd_error*0.01

@alias add c1 map icon -35.362502 149.162008 barrell
@alias add c2 map icon -35.363762 149.161739 barrell
@alias add c3 map icon -35.363772 149.161809 barrell
@alias add c4 map icon -35.365500 149.162207 barrell
@alias add c5 map icon -35.364812 149.164150 barrell
@alias add c6 map icon -35.365643 149.164326 barrell
@alias add ggimbal g degrees(GIMBAL_REPORT.joint_yaw) degrees(GIMBAL_REPORT.joint_roll) degrees(GIMBAL_REPORT.joint_pitch)
@alias add grpy312 g degrees(euler312(ATTITUDE).x) degrees(euler312(ATTITUDE).y) degrees(euler312(ATTITUDE).z)
@alias add gimutime g RAW_IMU.time_usec*1.0e-6
@alias add gekf g EKF_STATUS_REPORT.velocity_variance EKF_STATUS_REPORT.pos_horiz_variance EKF_STATUS_REPORT.pos_vert_variance EKF_STATUS_REPORT.compass_variance
@alias add gsimgyro g degrees(SIMSTATE.xgyro) degrees(SIMSTATE.ygyro) degrees(SIMSTATE.zgyro) degrees(RAW_IMU.xgyro*0.001) degrees(RAW_IMU.ygyro*0.001) degrees(RAW_IMU.zgyro*0.001)
@alias add gsimaccel g SIMSTATE.xacc SIMSTATE.yacc SIMSTATE.zacc RAW_IMU.xacc*9.81*0.001 RAW_IMU.yacc*9.81*0.001 RAW_IMU.zacc*9.81*0.001
@alias add gspeedup g lowpass(delta(RAW_IMU.time_usec*1.0e-6,"i"),"l",0.9)
@alias add gysim g degrees(SIMSTATE.yaw) degrees(ATTITUDE.yaw)

@alias add wp-cmac wp load ../Tools/autotest/CMAC-circuit.txt


#graph SERVO_OUTPUT_RAW.servo8_raw

# module load mavgraph

#@alias add x mavgraph ds add /home/pbarker/196.BIN
#@alias add y mavgraph graph ATT.Roll
#@alias add z mavgraph graph ATT.Pitch

# set state_basedir /home/pbarker/rc/flight-data

set moddebug 3

#module load dataflash_logger
#dataflash_logger set verbose true

#module load logger
#logger set verbose true
#logger set target_component 155

#module load map
#module load console
#module load speech

#module load avoidance

#module load cesium

# repeat add 1 status ADSB_VEHICLE

#module load loganalyzer

#module load example
module load msg

alias add fence-dalby fence load /home/pbarker/rc/ardupilot/Tools/autotest/ArduPlane-Missions/Dalby-OBC2016-fence.txt
# alias add wp-dalby wp load /home/pbarker/rc/ardupilot/Tools/autotest/ArduPlane-Missions/Dalby-OBC2016.txt
alias add wp-dalby wp load /home/pbarker/rc/cuav/script/OBC2016/mission-plane.txt

alias add wp-dalby-heli wp load /home/pbarker/rc/cuav/script/OBC2016/mission-heli.txt

#wpedit

#module load esp



alias add grcall graph RC_CHANNELS.chan1_raw RC_CHANNELS.chan2_raw RC_CHANNELS.chan3_raw RC_CHANNELS.chan4_raw RC_CHANNELS.chan5_raw RC_CHANNELS.chan6_raw RC_CHANNELS.chan7_raw RC_CHANNELS.chan8_raw RC_CHANNELS.chan9_raw RC_CHANNELS.chan10_raw RC_CHANNELS.chan11_raw RC_CHANNELS.chan12_raw

alias add grchigh graph SERVO_OUTPUT_RAW.servo7_raw SERVO_OUTPUT_RAW.servo8_raw SERVO_OUTPUT_RAW.servo9_raw SERVO_OUTPUT_RAW.servo10_raw SERVO_OUTPUT_RAW.servo11_raw SERVO_OUTPUT_RAW.servo12_raw

#graph degrees(ATTITUDE.yaw) wrap_180(NAV_CONTROLLER_OUTPUT.target_bearing)

#module load nsh

#gmag

#module load joystick
#module load mmap

#graph degrees(ATTITUDE.roll) degrees(ATTITUDE.pitch)

#module load videoscript
#avs set trigger_channel 7

module load devop
alias add old devop read i2c bob 0 0x52 0xc0 1

alias add newx devop i2c read 0 0x55 0 16
alias add new devop i2c read 1 0x1e 0x00 1

alias add i2cscan devop i2c scan 1
alias add spiread devop spi read mpu6000 0xf5 1

# DEVOP: 0x1e: 0

# module load ublox
#module load console

#module load video

#alias add o long DO_SEND_BANNER

#module load message

# set speech en+us-f3

alias add bob long set_message_interval
# graph diff(ATTITUDE.time_boot_ms,0)
# long SET_MESSAGE_INTERVAL 30 100000



