aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJulian Oes <julian@oes.ch>2013-05-17 11:24:02 +0200
committerJulian Oes <julian@oes.ch>2013-05-17 11:24:02 +0200
commitf5c157e74df12a0cb36b7d27cdad9828d96cc534 (patch)
tree3f758990921a7b52df8afe5131a8298b1141b6f4
parent80e8eeab2931e79e31adb17c93f5794e666c5763 (diff)
parentfa816d0fd65da461fd5bf8803cf00caebaf23c5c (diff)
downloadpx4-firmware-f5c157e74df12a0cb36b7d27cdad9828d96cc534.tar.gz
px4-firmware-f5c157e74df12a0cb36b7d27cdad9828d96cc534.tar.bz2
px4-firmware-f5c157e74df12a0cb36b7d27cdad9828d96cc534.zip
Merge remote-tracking branch 'upstream/master' into new_state_machine
Conflicts: src/drivers/px4io/px4io.cpp src/modules/commander/commander.c src/modules/commander/state_machine_helper.c
-rw-r--r--.gitignore7
-rw-r--r--Documentation/control_flow.graffle16650
-rw-r--r--Makefile268
-rw-r--r--ROMFS/.gitignore1
-rw-r--r--ROMFS/Makefile122
-rwxr-xr-xROMFS/logging/logconv.m224
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.FMU_quad_x (renamed from ROMFS/scripts/rc.FMU_quad_x)6
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.IO_QUAD107
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.PX4IO107
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.PX4IOAR (renamed from ROMFS/scripts/rc.PX4IOAR)37
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.boarddetect (renamed from ROMFS/scripts/rc.boarddetect)0
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.hil (renamed from ROMFS/scripts/rc.hil)17
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.jig (renamed from ROMFS/scripts/rc.jig)0
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.logging (renamed from ROMFS/scripts/rc.logging)0
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.sensors (renamed from ROMFS/scripts/rc.sensors)8
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.standalone (renamed from ROMFS/scripts/rc.standalone)0
-rw-r--r--ROMFS/px4fmu_common/init.d/rc.usb (renamed from ROMFS/scripts/rc.usb)0
-rwxr-xr-xROMFS/px4fmu_common/init.d/rcS (renamed from ROMFS/scripts/rcS)8
-rw-r--r--ROMFS/px4fmu_common/logging/logconv.m586
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_AERT.mix (renamed from ROMFS/mixers/FMU_AERT.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_AET.mix (renamed from ROMFS/mixers/FMU_AET.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_Q.mix (renamed from ROMFS/mixers/FMU_Q.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_RET.mix (renamed from ROMFS/mixers/FMU_RET.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_X5.mix (renamed from ROMFS/mixers/FMU_X5.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_delta.mix50
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_hex_+.mix (renamed from ROMFS/mixers/FMU_hex_+.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_hex_x.mix (renamed from ROMFS/mixers/FMU_hex_x.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_octo_+.mix (renamed from ROMFS/mixers/FMU_octo_+.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_octo_x.mix (renamed from ROMFS/mixers/FMU_octo_x.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_pass.mix (renamed from ROMFS/mixers/FMU_pass.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_quad_+.mix (renamed from ROMFS/mixers/FMU_quad_+.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_quad_v.mix7
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_quad_w.mix6
-rw-r--r--ROMFS/px4fmu_common/mixers/FMU_quad_x.mix (renamed from ROMFS/mixers/FMU_quad_x.mix)0
-rw-r--r--ROMFS/px4fmu_common/mixers/README (renamed from ROMFS/mixers/README)0
-rw-r--r--ROMFS/scripts/rc.PX4IO80
-rwxr-xr-xTools/px_uploader.py679
-rw-r--r--apps/drivers/l3gd20/Makefile42
-rw-r--r--apps/drivers/mpu6000/Makefile42
-rw-r--r--apps/drivers/stm32/adc/Makefile43
-rw-r--r--apps/drivers/stm32/tone_alarm/Makefile43
-rw-r--r--apps/examples/Makefile1
-rw-r--r--apps/examples/control_demo/Makefile42
-rw-r--r--apps/examples/kalman_demo/Makefile42
-rw-r--r--apps/examples/nsh/Makefile2
-rw-r--r--apps/examples/px4_deamon_app/Makefile42
-rw-r--r--apps/examples/px4_mavlink_debug/Makefile42
-rw-r--r--apps/examples/px4_simple_app/Makefile42
-rw-r--r--apps/fixedwing_att_control/Makefile45
-rw-r--r--apps/fixedwing_pos_control/Makefile45
-rw-r--r--apps/hott_telemetry/Makefile45
-rw-r--r--apps/mavlink/.context0
-rw-r--r--apps/mavlink_onboard/Makefile44
-rw-r--r--apps/mk/app.mk237
-rw-r--r--apps/multirotor_pos_control/Makefile42
-rw-r--r--apps/nshlib/Makefile2
-rw-r--r--apps/position_estimator/.context0
-rw-r--r--apps/px4/tests/.context0
-rw-r--r--apps/px4/tests/Makefile42
-rw-r--r--apps/sensors/.context0
-rw-r--r--apps/system/i2c/Makefile3
-rw-r--r--apps/systemcmds/bl_update/Makefile42
-rw-r--r--apps/systemcmds/boardinfo/.context0
-rw-r--r--apps/systemcmds/boardinfo/Makefile42
-rw-r--r--apps/systemcmds/calibration/Makefile42
-rwxr-xr-xapps/systemcmds/calibration/calibration.c147
-rwxr-xr-xapps/systemcmds/calibration/channels_cal.c196
-rwxr-xr-xapps/systemcmds/calibration/range_cal.c224
-rw-r--r--apps/systemcmds/calibration/servo_cal.c264
-rw-r--r--apps/systemcmds/delay_test/delay_test.c160
-rw-r--r--apps/systemcmds/eeprom/Makefile42
-rw-r--r--apps/systemcmds/perf/.context0
-rw-r--r--apps/systemcmds/preflight_check/Makefile42
-rw-r--r--apps/systemcmds/reboot/.context0
-rw-r--r--apps/systemcmds/reboot/Makefile42
-rw-r--r--apps/systemcmds/top/.context0
-rw-r--r--apps/systemcmds/top/Makefile42
-rw-r--r--apps/uORB/.context0
-rw-r--r--apps/uORB/Makefile42
-rw-r--r--makefiles/README.txt71
-rw-r--r--makefiles/board_px4fmu.mk10
-rw-r--r--makefiles/board_px4io.mk10
-rw-r--r--makefiles/config_px4fmu_default.mk127
-rw-r--r--makefiles/config_px4io_default.mk10
-rw-r--r--makefiles/firmware.mk450
-rw-r--r--makefiles/module.mk241
-rw-r--r--makefiles/nuttx.mk (renamed from apps/px4io/Makefile)58
-rw-r--r--makefiles/setup.mk91
-rw-r--r--makefiles/toolchain_gnu-arm-eabi.mk285
-rw-r--r--makefiles/upload.mk44
-rw-r--r--nuttx/arch/arm/src/stm32/stm32_otgfsdev.c1
-rwxr-xr-xnuttx/configs/px4fmu/include/board.h7
-rw-r--r--nuttx/configs/px4fmu/include/nsh_romfsimg.h42
-rw-r--r--nuttx/configs/px4fmu/nsh/appconfig91
-rwxr-xr-xnuttx/configs/px4fmu/nsh/defconfig6
-rw-r--r--nuttx/configs/px4io/common/Make.defs3
-rwxr-xr-xnuttx/configs/px4io/include/board.h11
-rw-r--r--nuttx/configs/px4io/io/appconfig8
-rw-r--r--nuttx/libc/stdio/lib_libdtoa.c32
-rw-r--r--nuttx/libc/stdio/lib_libvsprintf.c7
-rw-r--r--platforms/empty.c3
-rw-r--r--src/drivers/ardrone_interface/ardrone_interface.c (renamed from apps/ardrone_interface/ardrone_interface.c)0
-rw-r--r--src/drivers/ardrone_interface/ardrone_motor_control.c (renamed from apps/ardrone_interface/ardrone_motor_control.c)8
-rw-r--r--src/drivers/ardrone_interface/ardrone_motor_control.h (renamed from apps/ardrone_interface/ardrone_motor_control.h)0
-rw-r--r--src/drivers/ardrone_interface/module.mk40
-rw-r--r--src/drivers/blinkm/blinkm.cpp (renamed from apps/drivers/blinkm/blinkm.cpp)3
-rw-r--r--src/drivers/blinkm/module.mk (renamed from apps/drivers/device/Makefile)8
-rw-r--r--src/drivers/bma180/bma180.cpp (renamed from apps/drivers/bma180/bma180.cpp)0
-rw-r--r--src/drivers/bma180/module.mk (renamed from apps/drivers/bma180/Makefile)8
-rw-r--r--src/drivers/boards/px4fmu/module.mk10
-rw-r--r--src/drivers/boards/px4fmu/px4fmu_can.c (renamed from apps/drivers/boards/px4fmu/px4fmu_can.c)5
-rw-r--r--src/drivers/boards/px4fmu/px4fmu_init.c (renamed from apps/drivers/boards/px4fmu/px4fmu_init.c)60
-rw-r--r--src/drivers/boards/px4fmu/px4fmu_internal.h (renamed from apps/drivers/boards/px4fmu/px4fmu_internal.h)7
-rw-r--r--src/drivers/boards/px4fmu/px4fmu_led.c (renamed from apps/drivers/boards/px4fmu/px4fmu_led.c)28
-rw-r--r--src/drivers/boards/px4fmu/px4fmu_pwm_servo.c (renamed from apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c)0
-rw-r--r--src/drivers/boards/px4fmu/px4fmu_spi.c (renamed from apps/drivers/boards/px4fmu/px4fmu_spi.c)18
-rw-r--r--src/drivers/boards/px4fmu/px4fmu_usb.c (renamed from apps/drivers/boards/px4fmu/px4fmu_usb.c)0
-rw-r--r--src/drivers/boards/px4io/module.mk6
-rw-r--r--src/drivers/boards/px4io/px4io_init.c (renamed from apps/drivers/boards/px4io/px4io_init.c)8
-rw-r--r--src/drivers/boards/px4io/px4io_internal.h (renamed from apps/drivers/boards/px4io/px4io_internal.h)4
-rw-r--r--src/drivers/boards/px4io/px4io_pwm_servo.c (renamed from apps/drivers/boards/px4io/px4io_pwm_servo.c)0
-rw-r--r--src/drivers/device/cdev.cpp (renamed from apps/drivers/device/cdev.cpp)0
-rw-r--r--src/drivers/device/device.cpp (renamed from apps/drivers/device/device.cpp)0
-rw-r--r--src/drivers/device/device.h (renamed from apps/drivers/device/device.h)0
-rw-r--r--src/drivers/device/i2c.cpp (renamed from apps/drivers/device/i2c.cpp)0
-rw-r--r--src/drivers/device/i2c.h (renamed from apps/drivers/device/i2c.h)15
-rw-r--r--src/drivers/device/module.mk (renamed from apps/drivers/hmc5883/Makefile)14
-rw-r--r--src/drivers/device/pio.cpp (renamed from apps/drivers/device/pio.cpp)0
-rw-r--r--src/drivers/device/spi.cpp (renamed from apps/drivers/device/spi.cpp)0
-rw-r--r--src/drivers/device/spi.h (renamed from apps/drivers/device/spi.h)0
-rw-r--r--src/drivers/drv_accel.h (renamed from apps/drivers/drv_accel.h)0
-rw-r--r--src/drivers/drv_adc.h (renamed from apps/drivers/drv_adc.h)0
-rw-r--r--src/drivers/drv_airspeed.h61
-rw-r--r--src/drivers/drv_baro.h (renamed from apps/drivers/drv_baro.h)0
-rw-r--r--src/drivers/drv_blinkm.h (renamed from apps/drivers/drv_blinkm.h)0
-rw-r--r--src/drivers/drv_gpio.h (renamed from apps/drivers/drv_gpio.h)0
-rw-r--r--src/drivers/drv_gps.h (renamed from apps/drivers/drv_gps.h)0
-rw-r--r--src/drivers/drv_gyro.h (renamed from apps/drivers/drv_gyro.h)0
-rw-r--r--src/drivers/drv_hrt.h (renamed from apps/drivers/drv_hrt.h)0
-rw-r--r--src/drivers/drv_led.h (renamed from apps/drivers/drv_led.h)0
-rw-r--r--src/drivers/drv_mag.h (renamed from apps/drivers/drv_mag.h)0
-rw-r--r--src/drivers/drv_mixer.h (renamed from apps/drivers/drv_mixer.h)0
-rw-r--r--src/drivers/drv_orb_dev.h (renamed from apps/drivers/drv_orb_dev.h)2
-rw-r--r--src/drivers/drv_pwm_output.h (renamed from apps/drivers/drv_pwm_output.h)6
-rw-r--r--src/drivers/drv_range_finder.h (renamed from apps/drivers/drv_range_finder.h)0
-rw-r--r--src/drivers/drv_rc_input.h (renamed from apps/drivers/drv_rc_input.h)0
-rw-r--r--src/drivers/drv_sensor.h (renamed from apps/drivers/drv_sensor.h)0
-rw-r--r--src/drivers/drv_tone_alarm.h (renamed from apps/drivers/drv_tone_alarm.h)0
-rw-r--r--src/drivers/ets_airspeed/ets_airspeed.cpp832
-rw-r--r--src/drivers/ets_airspeed/module.mk41
-rw-r--r--src/drivers/gps/gps.cpp (renamed from apps/drivers/gps/gps.cpp)11
-rw-r--r--src/drivers/gps/gps_helper.cpp (renamed from apps/drivers/gps/gps_helper.cpp)34
-rw-r--r--src/drivers/gps/gps_helper.h (renamed from apps/drivers/gps/gps_helper.h)23
-rw-r--r--src/drivers/gps/module.mk (renamed from apps/drivers/gps/Makefile)11
-rw-r--r--src/drivers/gps/mtk.cpp (renamed from apps/drivers/gps/mtk.cpp)4
-rw-r--r--src/drivers/gps/mtk.h (renamed from apps/drivers/gps/mtk.h)6
-rw-r--r--src/drivers/gps/ubx.cpp (renamed from apps/drivers/gps/ubx.cpp)188
-rw-r--r--src/drivers/gps/ubx.h (renamed from apps/drivers/gps/ubx.h)78
-rw-r--r--src/drivers/hil/hil.cpp (renamed from apps/drivers/hil/hil.cpp)0
-rw-r--r--src/drivers/hil/module.mk40
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp (renamed from apps/drivers/hmc5883/hmc5883.cpp)50
-rw-r--r--src/drivers/hmc5883/module.mk (renamed from apps/drivers/hil/Makefile)13
-rw-r--r--src/drivers/hott_telemetry/hott_telemetry_main.c (renamed from apps/hott_telemetry/hott_telemetry_main.c)12
-rw-r--r--src/drivers/hott_telemetry/messages.c (renamed from apps/hott_telemetry/messages.c)18
-rw-r--r--src/drivers/hott_telemetry/messages.h (renamed from apps/hott_telemetry/messages.h)5
-rw-r--r--src/drivers/hott_telemetry/module.mk (renamed from apps/drivers/blinkm/Makefile)11
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp (renamed from apps/drivers/l3gd20/l3gd20.cpp)12
-rw-r--r--src/drivers/l3gd20/module.mk6
-rw-r--r--src/drivers/led/led.cpp (renamed from apps/drivers/led/led.cpp)21
-rw-r--r--src/drivers/led/module.mk (renamed from apps/systemlib/mixer/Makefile)7
-rw-r--r--src/drivers/mb12xx/mb12xx.cpp (renamed from apps/drivers/mb12xx/mb12xx.cpp)2
-rw-r--r--src/drivers/mb12xx/module.mk (renamed from apps/drivers/mb12xx/Makefile)8
-rw-r--r--src/drivers/md25/md25.cpp553
-rw-r--r--src/drivers/md25/md25.hpp293
-rw-r--r--src/drivers/md25/md25_main.cpp264
-rw-r--r--src/drivers/md25/module.mk (renamed from apps/ardrone_interface/Makefile)12
-rw-r--r--src/drivers/mkblctrl/mkblctrl.cpp1442
-rw-r--r--src/drivers/mkblctrl/module.mk (renamed from apps/commander/Makefile)13
-rw-r--r--src/drivers/mpu6000/module.mk (renamed from apps/mavlink/Makefile)13
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp (renamed from apps/drivers/mpu6000/mpu6000.cpp)2
-rw-r--r--src/drivers/ms5611/module.mk (renamed from apps/drivers/led/Makefile)8
-rw-r--r--src/drivers/ms5611/ms5611.cpp (renamed from apps/drivers/ms5611/ms5611.cpp)0
-rw-r--r--src/drivers/px4fmu/fmu.cpp (renamed from apps/drivers/px4fmu/fmu.cpp)11
-rw-r--r--src/drivers/px4fmu/module.mk6
-rw-r--r--src/drivers/px4io/module.mk (renamed from apps/drivers/px4io/Makefile)9
-rw-r--r--src/drivers/px4io/px4io.cpp (renamed from apps/drivers/px4io/px4io.cpp)124
-rw-r--r--src/drivers/px4io/uploader.cpp (renamed from apps/drivers/px4io/uploader.cpp)22
-rw-r--r--src/drivers/px4io/uploader.h (renamed from apps/drivers/px4io/uploader.h)5
-rw-r--r--src/drivers/stm32/adc/adc.cpp (renamed from apps/drivers/stm32/adc/adc.cpp)0
-rw-r--r--src/drivers/stm32/adc/module.mk42
-rw-r--r--src/drivers/stm32/drv_hrt.c (renamed from apps/drivers/stm32/drv_hrt.c)2
-rw-r--r--src/drivers/stm32/drv_pwm_servo.c (renamed from apps/drivers/stm32/drv_pwm_servo.c)2
-rw-r--r--src/drivers/stm32/drv_pwm_servo.h (renamed from apps/drivers/stm32/drv_pwm_servo.h)0
-rw-r--r--src/drivers/stm32/module.mk (renamed from apps/drivers/stm32/Makefile)7
-rw-r--r--src/drivers/stm32/tone_alarm/module.mk42
-rw-r--r--src/drivers/stm32/tone_alarm/tone_alarm.cpp (renamed from apps/drivers/stm32/tone_alarm/tone_alarm.cpp)8
-rw-r--r--src/examples/fixedwing_control/main.c477
-rw-r--r--src/examples/fixedwing_control/module.mk41
-rw-r--r--src/examples/fixedwing_control/params.c77
-rw-r--r--src/examples/fixedwing_control/params.h65
-rw-r--r--src/examples/math_demo/math_demo.cpp (renamed from apps/examples/math_demo/math_demo.cpp)5
-rw-r--r--src/examples/math_demo/module.mk (renamed from apps/drivers/ms5611/Makefile)11
-rw-r--r--src/examples/px4_daemon_app/module.mk40
-rw-r--r--src/examples/px4_daemon_app/px4_daemon_app.c (renamed from apps/examples/px4_deamon_app/px4_deamon_app.c)0
-rw-r--r--src/examples/px4_mavlink_debug/module.mk (renamed from apps/examples/math_demo/Makefile)8
-rw-r--r--src/examples/px4_mavlink_debug/px4_mavlink_debug.c (renamed from apps/examples/px4_mavlink_debug/px4_mavlink_debug.c)0
-rw-r--r--src/examples/px4_simple_app/module.mk40
-rw-r--r--src/examples/px4_simple_app/px4_simple_app.c (renamed from apps/examples/px4_simple_app/px4_simple_app.c)0
-rw-r--r--src/include/mavlink/mavlink_log.h (renamed from apps/mavlink/mavlink_log.h)0
-rw-r--r--src/include/visibility.h (renamed from apps/systemlib/visibility.h)0
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.cpp (renamed from apps/examples/kalman_demo/KalmanNav.cpp)2
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.hpp (renamed from apps/examples/kalman_demo/KalmanNav.hpp)0
-rw-r--r--src/modules/att_pos_estimator_ekf/kalman_main.cpp (renamed from apps/examples/kalman_demo/kalman_demo.cpp)4
-rw-r--r--src/modules/att_pos_estimator_ekf/module.mk (renamed from apps/drivers/px4fmu/Makefile)15
-rw-r--r--src/modules/att_pos_estimator_ekf/params.c (renamed from apps/examples/kalman_demo/params.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp (renamed from apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c)18
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c (renamed from apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h (renamed from apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c (renamed from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h (renamed from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c (renamed from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h (renamed from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c (renamed from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h (renamed from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h (renamed from apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/cross.c (renamed from apps/attitude_estimator_ekf/codegen/cross.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/cross.h (renamed from apps/attitude_estimator_ekf/codegen/cross.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/eye.c (renamed from apps/attitude_estimator_ekf/codegen/eye.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/eye.h (renamed from apps/attitude_estimator_ekf/codegen/eye.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/mrdivide.c (renamed from apps/attitude_estimator_ekf/codegen/mrdivide.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/mrdivide.h (renamed from apps/attitude_estimator_ekf/codegen/mrdivide.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/norm.c (renamed from apps/attitude_estimator_ekf/codegen/norm.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/norm.h (renamed from apps/attitude_estimator_ekf/codegen/norm.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/rdivide.c (renamed from apps/attitude_estimator_ekf/codegen/rdivide.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/rdivide.h (renamed from apps/attitude_estimator_ekf/codegen/rdivide.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/rtGetInf.c (renamed from apps/attitude_estimator_ekf/codegen/rtGetInf.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/rtGetInf.h (renamed from apps/attitude_estimator_ekf/codegen/rtGetInf.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/rtGetNaN.c (renamed from apps/attitude_estimator_ekf/codegen/rtGetNaN.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/rtGetNaN.h (renamed from apps/attitude_estimator_ekf/codegen/rtGetNaN.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/rt_defines.h (renamed from apps/attitude_estimator_ekf/codegen/rt_defines.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c (renamed from apps/attitude_estimator_ekf/codegen/rt_nonfinite.c)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h (renamed from apps/attitude_estimator_ekf/codegen/rt_nonfinite.h)0
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/codegen/rtwtypes.h (renamed from apps/attitude_estimator_ekf/codegen/rtwtypes.h)0
-rw-r--r--[-rwxr-xr-x]src/modules/attitude_estimator_ekf/module.mk (renamed from apps/attitude_estimator_ekf/Makefile)20
-rw-r--r--src/modules/commander/accelerometer_calibration.c404
-rw-r--r--src/modules/commander/accelerometer_calibration.h16
-rw-r--r--src/modules/commander/calibration_routines.c (renamed from apps/commander/calibration_routines.c)0
-rw-r--r--src/modules/commander/calibration_routines.h (renamed from apps/commander/calibration_routines.h)0
-rw-r--r--src/modules/commander/commander.c (renamed from apps/commander/commander.c)418
-rw-r--r--src/modules/commander/commander.h (renamed from apps/commander/commander.h)0
-rw-r--r--src/modules/commander/module.mk (renamed from apps/drivers/boards/px4fmu/Makefile)12
-rw-r--r--src/modules/commander/state_machine_helper.c (renamed from apps/commander/state_machine_helper.c)23
-rw-r--r--src/modules/commander/state_machine_helper.h (renamed from apps/commander/state_machine_helper.h)0
-rw-r--r--src/modules/controllib/block/Block.cpp (renamed from apps/controllib/block/Block.cpp)0
-rw-r--r--src/modules/controllib/block/Block.hpp (renamed from apps/controllib/block/Block.hpp)0
-rw-r--r--src/modules/controllib/block/BlockParam.cpp (renamed from apps/controllib/block/BlockParam.cpp)0
-rw-r--r--src/modules/controllib/block/BlockParam.hpp (renamed from apps/controllib/block/BlockParam.hpp)0
-rw-r--r--src/modules/controllib/block/List.hpp (renamed from apps/controllib/block/List.hpp)0
-rw-r--r--src/modules/controllib/block/UOrbPublication.cpp (renamed from apps/controllib/block/UOrbPublication.cpp)0
-rw-r--r--src/modules/controllib/block/UOrbPublication.hpp (renamed from apps/controllib/block/UOrbPublication.hpp)0
-rw-r--r--src/modules/controllib/block/UOrbSubscription.cpp (renamed from apps/controllib/block/UOrbSubscription.cpp)0
-rw-r--r--src/modules/controllib/block/UOrbSubscription.hpp (renamed from apps/controllib/block/UOrbSubscription.hpp)0
-rw-r--r--src/modules/controllib/blocks.cpp (renamed from apps/controllib/blocks.cpp)0
-rw-r--r--src/modules/controllib/blocks.hpp (renamed from apps/controllib/blocks.hpp)0
-rw-r--r--src/modules/controllib/fixedwing.cpp (renamed from apps/controllib/fixedwing.cpp)64
-rw-r--r--src/modules/controllib/fixedwing.hpp (renamed from apps/controllib/fixedwing.hpp)12
-rw-r--r--src/modules/controllib/module.mk (renamed from apps/controllib/Makefile)9
-rw-r--r--src/modules/controllib/test_params.c (renamed from apps/controllib/test_params.c)0
-rw-r--r--src/modules/fixedwing_att_control/fixedwing_att_control_att.c (renamed from apps/fixedwing_att_control/fixedwing_att_control_att.c)3
-rw-r--r--src/modules/fixedwing_att_control/fixedwing_att_control_att.h (renamed from apps/fixedwing_att_control/fixedwing_att_control_att.h)0
-rw-r--r--src/modules/fixedwing_att_control/fixedwing_att_control_main.c (renamed from apps/fixedwing_att_control/fixedwing_att_control_main.c)5
-rw-r--r--src/modules/fixedwing_att_control/fixedwing_att_control_rate.c (renamed from apps/fixedwing_att_control/fixedwing_att_control_rate.c)6
-rw-r--r--src/modules/fixedwing_att_control/fixedwing_att_control_rate.h (renamed from apps/fixedwing_att_control/fixedwing_att_control_rate.h)0
-rw-r--r--src/modules/fixedwing_att_control/module.mk42
-rw-r--r--src/modules/fixedwing_backside/fixedwing_backside_main.cpp (renamed from apps/examples/control_demo/control_demo.cpp)14
-rw-r--r--src/modules/fixedwing_backside/module.mk40
-rw-r--r--src/modules/fixedwing_backside/params.c (renamed from apps/examples/control_demo/params.c)17
-rw-r--r--src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c (renamed from apps/fixedwing_pos_control/fixedwing_pos_control_main.c)0
-rw-r--r--src/modules/fixedwing_pos_control/module.mk (renamed from apps/systemcmds/delay_test/Makefile)10
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/CommonTables/arm_common_tables.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/CommonTables/arm_common_tables.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_opt_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_opt_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_opt_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_opt_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_opt_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_opt_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_inverse_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_inverse_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_float.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_float.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_float.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_float.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q7.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q7.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_float.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_float.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_bitreversal.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_bitreversal.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_f32.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_f32.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q15.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q15.c)0
-rw-r--r--src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q31.c (renamed from apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q31.c)0
-rw-r--r--src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h (renamed from apps/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h)0
-rw-r--r--src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h (renamed from apps/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h)0
-rw-r--r--src/modules/mathlib/CMSIS/Include/arm_common_tables.h (renamed from apps/mathlib/CMSIS/Include/arm_common_tables.h)0
-rw-r--r--src/modules/mathlib/CMSIS/Include/arm_math.h (renamed from apps/mathlib/CMSIS/Include/arm_math.h)0
-rw-r--r--src/modules/mathlib/CMSIS/Include/core_cm3.h (renamed from apps/mathlib/CMSIS/Include/core_cm3.h)0
-rw-r--r--src/modules/mathlib/CMSIS/Include/core_cm4.h (renamed from apps/mathlib/CMSIS/Include/core_cm4.h)0
-rw-r--r--src/modules/mathlib/CMSIS/Include/core_cm4_simd.h (renamed from apps/mathlib/CMSIS/Include/core_cm4_simd.h)0
-rw-r--r--src/modules/mathlib/CMSIS/Include/core_cmFunc.h (renamed from apps/mathlib/CMSIS/Include/core_cmFunc.h)0
-rw-r--r--src/modules/mathlib/CMSIS/Include/core_cmInstr.h (renamed from apps/mathlib/CMSIS/Include/core_cmInstr.h)0
-rw-r--r--src/modules/mathlib/CMSIS/module.mk (renamed from apps/mathlib/CMSIS/Makefile)9
-rw-r--r--src/modules/mathlib/math/Dcm.cpp (renamed from apps/mathlib/math/Dcm.cpp)2
-rw-r--r--src/modules/mathlib/math/Dcm.hpp (renamed from apps/mathlib/math/Dcm.hpp)0
-rw-r--r--src/modules/mathlib/math/EulerAngles.cpp (renamed from apps/mathlib/math/EulerAngles.cpp)2
-rw-r--r--src/modules/mathlib/math/EulerAngles.hpp (renamed from apps/mathlib/math/EulerAngles.hpp)0
-rw-r--r--src/modules/mathlib/math/Matrix.cpp (renamed from apps/mathlib/math/Matrix.cpp)2
-rw-r--r--src/modules/mathlib/math/Matrix.hpp (renamed from apps/mathlib/math/Matrix.hpp)0
-rw-r--r--src/modules/mathlib/math/Quaternion.cpp (renamed from apps/mathlib/math/Quaternion.cpp)2
-rw-r--r--src/modules/mathlib/math/Quaternion.hpp (renamed from apps/mathlib/math/Quaternion.hpp)0
-rw-r--r--src/modules/mathlib/math/Vector.cpp (renamed from apps/mathlib/math/Vector.cpp)2
-rw-r--r--src/modules/mathlib/math/Vector.hpp (renamed from apps/mathlib/math/Vector.hpp)0
-rw-r--r--src/modules/mathlib/math/Vector3.cpp (renamed from apps/mathlib/math/Vector3.cpp)2
-rw-r--r--src/modules/mathlib/math/Vector3.hpp (renamed from apps/mathlib/math/Vector3.hpp)0
-rw-r--r--src/modules/mathlib/math/arm/Matrix.cpp (renamed from apps/mathlib/math/arm/Matrix.cpp)0
-rw-r--r--src/modules/mathlib/math/arm/Matrix.hpp (renamed from apps/mathlib/math/arm/Matrix.hpp)0
-rw-r--r--src/modules/mathlib/math/arm/Vector.cpp (renamed from apps/mathlib/math/arm/Vector.cpp)0
-rw-r--r--src/modules/mathlib/math/arm/Vector.hpp (renamed from apps/mathlib/math/arm/Vector.hpp)0
-rw-r--r--src/modules/mathlib/math/generic/Matrix.cpp (renamed from apps/mathlib/math/generic/Matrix.cpp)0
-rw-r--r--src/modules/mathlib/math/generic/Matrix.hpp (renamed from apps/mathlib/math/generic/Matrix.hpp)0
-rw-r--r--src/modules/mathlib/math/generic/Vector.cpp (renamed from apps/mathlib/math/generic/Vector.cpp)0
-rw-r--r--src/modules/mathlib/math/generic/Vector.hpp (renamed from apps/mathlib/math/generic/Vector.hpp)0
-rw-r--r--src/modules/mathlib/math/nasa_rotation_def.pdf (renamed from apps/mathlib/math/nasa_rotation_def.pdf)bin709235 -> 709235 bytes
-rw-r--r--src/modules/mathlib/math/test/test.cpp (renamed from apps/mathlib/math/test/test.cpp)1
-rw-r--r--src/modules/mathlib/math/test/test.hpp (renamed from apps/mathlib/math/test/test.hpp)6
-rw-r--r--src/modules/mathlib/math/test_math.sce (renamed from apps/mathlib/math/test_math.sce)0
-rw-r--r--src/modules/mathlib/mathlib.h (renamed from apps/mathlib/mathlib.h)0
-rw-r--r--src/modules/mathlib/module.mk (renamed from apps/mathlib/Makefile)16
-rw-r--r--src/modules/mavlink/.gitignore (renamed from apps/mavlink/.gitignore)0
-rw-r--r--src/modules/mavlink/mavlink.c (renamed from apps/mavlink/mavlink.c)2
-rw-r--r--src/modules/mavlink/mavlink_bridge_header.h (renamed from apps/mavlink/mavlink_bridge_header.h)2
-rw-r--r--src/modules/mavlink/mavlink_hil.h (renamed from apps/mavlink/mavlink_hil.h)0
-rw-r--r--src/modules/mavlink/mavlink_log.c (renamed from apps/mavlink/mavlink_log.c)2
-rw-r--r--src/modules/mavlink/mavlink_parameters.c (renamed from apps/mavlink/mavlink_parameters.c)0
-rw-r--r--src/modules/mavlink/mavlink_parameters.h (renamed from apps/mavlink/mavlink_parameters.h)0
-rw-r--r--src/modules/mavlink/mavlink_receiver.c (renamed from apps/mavlink/mavlink_receiver.c)124
-rw-r--r--src/modules/mavlink/missionlib.c (renamed from apps/mavlink/missionlib.c)2
-rw-r--r--src/modules/mavlink/missionlib.h (renamed from apps/mavlink/missionlib.h)0
-rw-r--r--src/modules/mavlink/module.mk47
-rw-r--r--src/modules/mavlink/orb_listener.c (renamed from apps/mavlink/orb_listener.c)21
-rw-r--r--src/modules/mavlink/orb_topics.h (renamed from apps/mavlink/orb_topics.h)0
-rw-r--r--src/modules/mavlink/util.h (renamed from apps/mavlink/util.h)0
-rw-r--r--src/modules/mavlink/waypoints.c (renamed from apps/mavlink/waypoints.c)0
-rw-r--r--src/modules/mavlink/waypoints.h (renamed from apps/mavlink/waypoints.h)0
-rw-r--r--src/modules/mavlink_onboard/mavlink.c (renamed from apps/mavlink_onboard/mavlink.c)0
-rw-r--r--src/modules/mavlink_onboard/mavlink_bridge_header.h (renamed from apps/mavlink_onboard/mavlink_bridge_header.h)2
-rw-r--r--src/modules/mavlink_onboard/mavlink_receiver.c (renamed from apps/mavlink_onboard/mavlink_receiver.c)0
-rw-r--r--src/modules/mavlink_onboard/module.mk42
-rw-r--r--src/modules/mavlink_onboard/orb_topics.h (renamed from apps/mavlink_onboard/orb_topics.h)0
-rw-r--r--src/modules/mavlink_onboard/util.h (renamed from apps/mavlink_onboard/util.h)0
-rwxr-xr-xsrc/modules/multirotor_att_control/module.mk42
-rw-r--r--src/modules/multirotor_att_control/multirotor_att_control_main.c (renamed from apps/multirotor_att_control/multirotor_att_control_main.c)0
-rw-r--r--src/modules/multirotor_att_control/multirotor_attitude_control.c (renamed from apps/multirotor_att_control/multirotor_attitude_control.c)0
-rw-r--r--src/modules/multirotor_att_control/multirotor_attitude_control.h (renamed from apps/multirotor_att_control/multirotor_attitude_control.h)0
-rw-r--r--src/modules/multirotor_att_control/multirotor_rate_control.c (renamed from apps/multirotor_att_control/multirotor_rate_control.c)0
-rw-r--r--src/modules/multirotor_att_control/multirotor_rate_control.h (renamed from apps/multirotor_att_control/multirotor_rate_control.h)0
-rw-r--r--src/modules/multirotor_pos_control/module.mk41
-rw-r--r--src/modules/multirotor_pos_control/multirotor_pos_control.c (renamed from apps/multirotor_pos_control/multirotor_pos_control.c)0
-rw-r--r--src/modules/multirotor_pos_control/multirotor_pos_control_params.c (renamed from apps/multirotor_pos_control/multirotor_pos_control_params.c)0
-rw-r--r--src/modules/multirotor_pos_control/multirotor_pos_control_params.h (renamed from apps/multirotor_pos_control/multirotor_pos_control_params.h)0
-rw-r--r--src/modules/multirotor_pos_control/position_control.c (renamed from apps/multirotor_pos_control/position_control.c)0
-rw-r--r--src/modules/multirotor_pos_control/position_control.h (renamed from apps/multirotor_pos_control/position_control.h)0
-rw-r--r--src/modules/position_estimator/module.mk (renamed from apps/position_estimator/Makefile)12
-rw-r--r--src/modules/position_estimator/position_estimator_main.c (renamed from apps/position_estimator/position_estimator_main.c)0
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe1.c58
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe1.h30
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h30
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h30
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h16
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe2.c119
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe2.h32
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h32
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h32
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h16
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe3.c137
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe3.h33
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c32
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h38
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c47
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h33
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h33
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h16
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c136
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c157
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h16
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h31
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h16
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/randn.c524
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/randn.h33
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/rtGetInf.c139
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/rtGetInf.h23
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/rtGetNaN.c96
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/rtGetNaN.h21
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/rt_nonfinite.c87
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/rt_nonfinite.h53
-rwxr-xr-xsrc/modules/position_estimator_mc/codegen/rtwtypes.h159
-rwxr-xr-xsrc/modules/position_estimator_mc/kalman_dlqe1.m3
-rwxr-xr-xsrc/modules/position_estimator_mc/kalman_dlqe2.m9
-rwxr-xr-xsrc/modules/position_estimator_mc/kalman_dlqe3.m17
-rw-r--r--src/modules/position_estimator_mc/module.mk60
-rwxr-xr-xsrc/modules/position_estimator_mc/positionKalmanFilter1D.m19
-rwxr-xr-xsrc/modules/position_estimator_mc/positionKalmanFilter1D_dT.m26
-rwxr-xr-xsrc/modules/position_estimator_mc/position_estimator_mc_main.c510
-rwxr-xr-xsrc/modules/position_estimator_mc/position_estimator_mc_params.c68
-rwxr-xr-xsrc/modules/position_estimator_mc/position_estimator_mc_params.h69
-rw-r--r--src/modules/px4iofirmware/adc.c (renamed from apps/px4io/adc.c)10
-rw-r--r--src/modules/px4iofirmware/controls.c (renamed from apps/px4io/controls.c)0
-rw-r--r--src/modules/px4iofirmware/dsm.c (renamed from apps/px4io/dsm.c)0
-rw-r--r--src/modules/px4iofirmware/i2c.c (renamed from apps/px4io/i2c.c)0
-rw-r--r--src/modules/px4iofirmware/mixer.cpp (renamed from apps/px4io/mixer.cpp)4
-rw-r--r--src/modules/px4iofirmware/module.mk19
-rw-r--r--src/modules/px4iofirmware/protocol.h (renamed from apps/px4io/protocol.h)8
-rw-r--r--src/modules/px4iofirmware/px4io.c (renamed from apps/px4io/px4io.c)6
-rw-r--r--src/modules/px4iofirmware/px4io.h (renamed from apps/px4io/px4io.h)21
-rw-r--r--src/modules/px4iofirmware/registers.c (renamed from apps/px4io/registers.c)36
-rw-r--r--src/modules/px4iofirmware/safety.c (renamed from apps/px4io/safety.c)30
-rw-r--r--src/modules/px4iofirmware/sbus.c (renamed from apps/px4io/sbus.c)0
-rw-r--r--src/modules/sdlog/module.mk (renamed from apps/sdlog/Makefile)10
-rw-r--r--src/modules/sdlog/sdlog.c (renamed from apps/sdlog/sdlog.c)27
-rw-r--r--src/modules/sdlog/sdlog_ringbuffer.c (renamed from apps/sdlog/sdlog_ringbuffer.c)0
-rw-r--r--src/modules/sdlog/sdlog_ringbuffer.h (renamed from apps/sdlog/sdlog_ringbuffer.h)0
-rw-r--r--src/modules/sensors/.context (renamed from apps/commander/.context)0
-rw-r--r--src/modules/sensors/module.mk (renamed from apps/sensors/Makefile)13
-rw-r--r--src/modules/sensors/sensor_params.c (renamed from apps/sensors/sensor_params.c)2
-rw-r--r--src/modules/sensors/sensors.cpp (renamed from apps/sensors/sensors.cpp)91
-rw-r--r--src/modules/systemlib/airspeed.c (renamed from apps/systemlib/airspeed.c)2
-rw-r--r--src/modules/systemlib/airspeed.h (renamed from apps/systemlib/airspeed.h)0
-rw-r--r--src/modules/systemlib/bson/tinybson.c (renamed from apps/systemlib/bson/tinybson.c)2
-rw-r--r--src/modules/systemlib/bson/tinybson.h (renamed from apps/systemlib/bson/tinybson.h)0
-rw-r--r--src/modules/systemlib/conversions.c (renamed from apps/systemlib/conversions.c)0
-rw-r--r--src/modules/systemlib/conversions.h (renamed from apps/systemlib/conversions.h)0
-rw-r--r--src/modules/systemlib/cpuload.c (renamed from apps/systemlib/cpuload.c)0
-rw-r--r--src/modules/systemlib/cpuload.h (renamed from apps/systemlib/cpuload.h)0
-rw-r--r--src/modules/systemlib/err.c (renamed from apps/systemlib/err.c)0
-rw-r--r--src/modules/systemlib/err.h (renamed from apps/systemlib/err.h)0
-rw-r--r--src/modules/systemlib/geo/geo.c (renamed from apps/systemlib/geo/geo.c)0
-rw-r--r--src/modules/systemlib/geo/geo.h (renamed from apps/systemlib/geo/geo.h)0
-rw-r--r--src/modules/systemlib/getopt_long.c (renamed from apps/systemlib/getopt_long.c)0
-rw-r--r--src/modules/systemlib/getopt_long.h (renamed from apps/systemlib/getopt_long.h)0
-rw-r--r--src/modules/systemlib/hx_stream.c (renamed from apps/systemlib/hx_stream.c)0
-rw-r--r--src/modules/systemlib/hx_stream.h (renamed from apps/systemlib/hx_stream.h)0
-rw-r--r--src/modules/systemlib/mixer/mixer.cpp (renamed from apps/systemlib/mixer/mixer.cpp)0
-rw-r--r--src/modules/systemlib/mixer/mixer.h (renamed from apps/systemlib/mixer/mixer.h)2
-rw-r--r--src/modules/systemlib/mixer/mixer_group.cpp (renamed from apps/systemlib/mixer/mixer_group.cpp)0
-rw-r--r--src/modules/systemlib/mixer/mixer_multirotor.cpp (renamed from apps/systemlib/mixer/mixer_multirotor.cpp)22
-rw-r--r--src/modules/systemlib/mixer/mixer_simple.cpp (renamed from apps/systemlib/mixer/mixer_simple.cpp)0
-rw-r--r--src/modules/systemlib/mixer/module.mk42
-rwxr-xr-xsrc/modules/systemlib/mixer/multi_tables (renamed from apps/systemlib/mixer/multi_tables)16
-rw-r--r--src/modules/systemlib/module.mk (renamed from apps/systemlib/Makefile)18
-rw-r--r--src/modules/systemlib/param/param.c (renamed from apps/systemlib/param/param.c)2
-rw-r--r--src/modules/systemlib/param/param.h (renamed from apps/systemlib/param/param.h)0
-rw-r--r--src/modules/systemlib/perf_counter.c (renamed from apps/systemlib/perf_counter.c)0
-rw-r--r--src/modules/systemlib/perf_counter.h (renamed from apps/systemlib/perf_counter.h)0
-rw-r--r--src/modules/systemlib/pid/pid.c (renamed from apps/systemlib/pid/pid.c)0
-rw-r--r--src/modules/systemlib/pid/pid.h (renamed from apps/systemlib/pid/pid.h)0
-rw-r--r--src/modules/systemlib/ppm_decode.c (renamed from apps/systemlib/ppm_decode.c)0
-rw-r--r--src/modules/systemlib/ppm_decode.h (renamed from apps/systemlib/ppm_decode.h)0
-rw-r--r--src/modules/systemlib/scheduling_priorities.h (renamed from apps/systemlib/scheduling_priorities.h)0
-rw-r--r--src/modules/systemlib/systemlib.c (renamed from apps/systemlib/systemlib.c)0
-rw-r--r--src/modules/systemlib/systemlib.h (renamed from apps/systemlib/systemlib.h)0
-rw-r--r--src/modules/systemlib/up_cxxinitialize.c (renamed from apps/systemlib/up_cxxinitialize.c)0
-rw-r--r--src/modules/systemlib/uthash/doc/userguide.txt (renamed from apps/systemlib/uthash/doc/userguide.txt)0
-rw-r--r--src/modules/systemlib/uthash/doc/utarray.txt (renamed from apps/systemlib/uthash/doc/utarray.txt)0
-rw-r--r--src/modules/systemlib/uthash/doc/utlist.txt (renamed from apps/systemlib/uthash/doc/utlist.txt)0
-rw-r--r--src/modules/systemlib/uthash/doc/utstring.txt (renamed from apps/systemlib/uthash/doc/utstring.txt)0
-rw-r--r--src/modules/systemlib/uthash/utarray.h (renamed from apps/systemlib/uthash/utarray.h)0
-rw-r--r--src/modules/systemlib/uthash/uthash.h (renamed from apps/systemlib/uthash/uthash.h)0
-rw-r--r--src/modules/systemlib/uthash/utlist.h (renamed from apps/systemlib/uthash/utlist.h)0
-rw-r--r--src/modules/systemlib/uthash/utstring.h (renamed from apps/systemlib/uthash/utstring.h)0
-rw-r--r--src/modules/systemlib/visibility.h (renamed from apps/systemcmds/calibration/calibration.h)48
-rw-r--r--src/modules/test/foo.c4
-rw-r--r--src/modules/test/module.mk4
-rw-r--r--[-rwxr-xr-x]src/modules/uORB/module.mk (renamed from apps/multirotor_att_control/Makefile)12
-rw-r--r--src/modules/uORB/objects_common.cpp (renamed from apps/uORB/objects_common.cpp)3
-rw-r--r--src/modules/uORB/topics/actuator_controls.h (renamed from apps/uORB/topics/actuator_controls.h)1
-rw-r--r--src/modules/uORB/topics/actuator_controls_effective.h (renamed from apps/uORB/topics/actuator_controls_effective.h)0
-rw-r--r--src/modules/uORB/topics/actuator_outputs.h (renamed from apps/uORB/topics/actuator_outputs.h)0
-rw-r--r--src/modules/uORB/topics/airspeed.h67
-rw-r--r--src/modules/uORB/topics/battery_status.h (renamed from apps/uORB/topics/battery_status.h)0
-rw-r--r--src/modules/uORB/topics/debug_key_value.h (renamed from apps/uORB/topics/debug_key_value.h)0
-rw-r--r--src/modules/uORB/topics/differential_pressure.h (renamed from apps/uORB/topics/differential_pressure.h)14
-rw-r--r--src/modules/uORB/topics/home_position.h (renamed from apps/uORB/topics/home_position.h)0
-rw-r--r--src/modules/uORB/topics/manual_control_setpoint.h (renamed from apps/uORB/topics/manual_control_setpoint.h)0
-rw-r--r--src/modules/uORB/topics/offboard_control_setpoint.h (renamed from apps/uORB/topics/offboard_control_setpoint.h)0
-rw-r--r--src/modules/uORB/topics/omnidirectional_flow.h (renamed from apps/uORB/topics/omnidirectional_flow.h)0
-rw-r--r--src/modules/uORB/topics/optical_flow.h (renamed from apps/uORB/topics/optical_flow.h)0
-rw-r--r--src/modules/uORB/topics/parameter_update.h (renamed from apps/uORB/topics/parameter_update.h)0
-rw-r--r--src/modules/uORB/topics/rc_channels.h (renamed from apps/uORB/topics/rc_channels.h)0
-rw-r--r--src/modules/uORB/topics/sensor_combined.h (renamed from apps/uORB/topics/sensor_combined.h)2
-rw-r--r--src/modules/uORB/topics/subsystem_info.h (renamed from apps/uORB/topics/subsystem_info.h)0
-rwxr-xr-xsrc/modules/uORB/topics/vehicle_attitude.h (renamed from apps/uORB/topics/vehicle_attitude.h)0
-rw-r--r--src/modules/uORB/topics/vehicle_attitude_setpoint.h (renamed from apps/uORB/topics/vehicle_attitude_setpoint.h)0
-rw-r--r--src/modules/uORB/topics/vehicle_command.h (renamed from apps/uORB/topics/vehicle_command.h)0
-rw-r--r--src/modules/uORB/topics/vehicle_global_position.h (renamed from apps/uORB/topics/vehicle_global_position.h)0
-rw-r--r--src/modules/uORB/topics/vehicle_global_position_set_triplet.h (renamed from apps/uORB/topics/vehicle_global_position_set_triplet.h)0
-rw-r--r--src/modules/uORB/topics/vehicle_global_position_setpoint.h (renamed from apps/uORB/topics/vehicle_global_position_setpoint.h)0
-rw-r--r--src/modules/uORB/topics/vehicle_gps_position.h (renamed from apps/uORB/topics/vehicle_gps_position.h)39
-rw-r--r--src/modules/uORB/topics/vehicle_local_position.h (renamed from apps/uORB/topics/vehicle_local_position.h)0
-rw-r--r--src/modules/uORB/topics/vehicle_local_position_setpoint.h (renamed from apps/uORB/topics/vehicle_local_position_setpoint.h)0
-rw-r--r--src/modules/uORB/topics/vehicle_rates_setpoint.h (renamed from apps/uORB/topics/vehicle_rates_setpoint.h)0
-rw-r--r--src/modules/uORB/topics/vehicle_status.h (renamed from apps/uORB/topics/vehicle_status.h)0
-rw-r--r--src/modules/uORB/topics/vehicle_vicon_position.h (renamed from apps/uORB/topics/vehicle_vicon_position.h)0
-rw-r--r--src/modules/uORB/uORB.cpp (renamed from apps/uORB/uORB.cpp)0
-rw-r--r--src/modules/uORB/uORB.h (renamed from apps/uORB/uORB.h)0
-rw-r--r--src/systemcmds/bl_update/bl_update.c (renamed from apps/systemcmds/bl_update/bl_update.c)2
-rw-r--r--src/systemcmds/bl_update/module.mk (renamed from apps/drivers/boards/px4io/Makefile)12
-rw-r--r--src/systemcmds/boardinfo/boardinfo.c (renamed from apps/systemcmds/boardinfo/boardinfo.c)2
-rw-r--r--src/systemcmds/boardinfo/module.mk41
-rw-r--r--src/systemcmds/eeprom/24xxxx_mtd.c (renamed from apps/systemcmds/eeprom/24xxxx_mtd.c)0
-rw-r--r--src/systemcmds/eeprom/eeprom.c (renamed from apps/systemcmds/eeprom/eeprom.c)0
-rw-r--r--src/systemcmds/eeprom/module.mk39
-rw-r--r--src/systemcmds/i2c/i2c.c (renamed from apps/systemcmds/i2c/i2c.c)2
-rw-r--r--src/systemcmds/i2c/module.mk (renamed from apps/systemcmds/i2c/Makefile)9
-rw-r--r--src/systemcmds/mixer/mixer.c (renamed from apps/systemcmds/mixer/mixer.c)0
-rw-r--r--src/systemcmds/mixer/module.mk (renamed from apps/systemcmds/mixer/Makefile)11
-rw-r--r--src/systemcmds/param/module.mk (renamed from apps/systemcmds/param/Makefile)12
-rw-r--r--src/systemcmds/param/param.c (renamed from apps/systemcmds/param/param.c)20
-rw-r--r--src/systemcmds/perf/module.mk (renamed from apps/systemcmds/perf/Makefile)9
-rw-r--r--src/systemcmds/perf/perf.c (renamed from apps/systemcmds/perf/perf.c)2
-rw-r--r--src/systemcmds/preflight_check/module.mk42
-rw-r--r--src/systemcmds/preflight_check/preflight_check.c (renamed from apps/systemcmds/preflight_check/preflight_check.c)2
-rw-r--r--src/systemcmds/pwm/module.mk (renamed from apps/systemcmds/pwm/Makefile)9
-rw-r--r--src/systemcmds/pwm/pwm.c (renamed from apps/systemcmds/pwm/pwm.c)19
-rw-r--r--src/systemcmds/reboot/module.mk41
-rw-r--r--src/systemcmds/reboot/reboot.c (renamed from apps/systemcmds/reboot/reboot.c)0
-rw-r--r--src/systemcmds/tests/.context (renamed from apps/examples/cdcacm/.context)0
-rw-r--r--src/systemcmds/tests/module.mk28
-rw-r--r--src/systemcmds/tests/test_adc.c (renamed from apps/px4/tests/test_adc.c)0
-rw-r--r--src/systemcmds/tests/test_bson.c (renamed from apps/px4/tests/test_bson.c)0
-rw-r--r--src/systemcmds/tests/test_float.c (renamed from apps/px4/tests/test_float.c)0
-rw-r--r--src/systemcmds/tests/test_gpio.c (renamed from apps/px4/tests/test_gpio.c)0
-rw-r--r--src/systemcmds/tests/test_hott_telemetry.c (renamed from apps/px4/tests/test_hott_telemetry.c)1
-rw-r--r--src/systemcmds/tests/test_hrt.c (renamed from apps/px4/tests/test_hrt.c)0
-rw-r--r--src/systemcmds/tests/test_int.c (renamed from apps/px4/tests/test_int.c)0
-rw-r--r--src/systemcmds/tests/test_jig_voltages.c (renamed from apps/px4/tests/test_jig_voltages.c)0
-rw-r--r--src/systemcmds/tests/test_led.c (renamed from apps/px4/tests/test_led.c)0
-rw-r--r--src/systemcmds/tests/test_sensors.c (renamed from apps/px4/tests/test_sensors.c)0
-rw-r--r--src/systemcmds/tests/test_servo.c (renamed from apps/px4/tests/test_servo.c)0
-rw-r--r--src/systemcmds/tests/test_sleep.c (renamed from apps/px4/tests/test_sleep.c)0
-rw-r--r--src/systemcmds/tests/test_time.c (renamed from apps/px4/tests/test_time.c)0
-rw-r--r--src/systemcmds/tests/test_uart_baudchange.c (renamed from apps/px4/tests/test_uart_baudchange.c)0
-rw-r--r--src/systemcmds/tests/test_uart_console.c (renamed from apps/px4/tests/test_uart_console.c)0
-rw-r--r--src/systemcmds/tests/test_uart_loopback.c (renamed from apps/px4/tests/test_uart_loopback.c)0
-rw-r--r--src/systemcmds/tests/test_uart_send.c (renamed from apps/px4/tests/test_uart_send.c)0
-rw-r--r--src/systemcmds/tests/tests.h (renamed from apps/px4/tests/tests.h)0
-rw-r--r--src/systemcmds/tests/tests_file.c (renamed from apps/px4/tests/tests_file.c)0
-rw-r--r--src/systemcmds/tests/tests_main.c (renamed from apps/px4/tests/tests_main.c)0
-rw-r--r--src/systemcmds/tests/tests_param.c (renamed from apps/px4/tests/tests_param.c)0
-rw-r--r--src/systemcmds/top/module.mk44
-rw-r--r--src/systemcmds/top/top.c (renamed from apps/systemcmds/top/top.c)0
807 files changed, 29867 insertions, 4245 deletions
diff --git a/.gitignore b/.gitignore
index 8e9075ba4..de03b0a60 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,5 @@
.built
+.context
*.context
*.bdat
*.pdat
@@ -43,7 +44,6 @@ nuttx/nuttx.hex
.settings
Firmware.sublime-workspace
.DS_Store
-nsh_romfsimg.h
cscope.out
.configX-e
nuttx-export.zip
@@ -55,3 +55,8 @@ mavlink/include/mavlink/v0.9/
core
.gdbinit
mkdeps
+Archives
+Build
+!ROMFS/*/*.d
+!ROMFS/*/*/*.d
+!ROMFS/*/*/*/*.d
diff --git a/Documentation/control_flow.graffle b/Documentation/control_flow.graffle
new file mode 100644
index 000000000..253523110
--- /dev/null
+++ b/Documentation/control_flow.graffle
@@ -0,0 +1,16650 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+ <key>ApplicationVersion</key>
+ <array>
+ <string>com.omnigroup.OmniGraffle</string>
+ <string>139.16.0.171715</string>
+ </array>
+ <key>CreationDate</key>
+ <string>2013-02-22 17:51:02 +0000</string>
+ <key>Creator</key>
+ <string>Lorenz Meier</string>
+ <key>GraphDocumentVersion</key>
+ <integer>8</integer>
+ <key>GuidesLocked</key>
+ <string>NO</string>
+ <key>GuidesVisible</key>
+ <string>YES</string>
+ <key>ImageCounter</key>
+ <integer>1</integer>
+ <key>LinksVisible</key>
+ <string>NO</string>
+ <key>MagnetsVisible</key>
+ <string>NO</string>
+ <key>MasterSheets</key>
+ <array/>
+ <key>ModificationDate</key>
+ <string>2013-04-20 15:38:47 +0000</string>
+ <key>Modifier</key>
+ <string>Lorenz Meier</string>
+ <key>NotesVisible</key>
+ <string>NO</string>
+ <key>OriginVisible</key>
+ <string>NO</string>
+ <key>PageBreaks</key>
+ <string>YES</string>
+ <key>PrintInfo</key>
+ <dict>
+ <key>NSBottomMargin</key>
+ <array>
+ <string>float</string>
+ <string>41</string>
+ </array>
+ <key>NSHorizonalPagination</key>
+ <array>
+ <string>coded</string>
+ <string>BAtzdHJlYW10eXBlZIHoA4QBQISEhAhOU051bWJlcgCEhAdOU1ZhbHVlAISECE5TT2JqZWN0AIWEASqEhAFxlwCG</string>
+ </array>
+ <key>NSLeftMargin</key>
+ <array>
+ <string>float</string>
+ <string>18</string>
+ </array>
+ <key>NSPaperName</key>
+ <array>
+ <string>string</string>
+ <string>iso-a3</string>
+ </array>
+ <key>NSPaperSize</key>
+ <array>
+ <string>size</string>
+ <string>{842, 1191}</string>
+ </array>
+ <key>NSPrintReverseOrientation</key>
+ <array>
+ <string>int</string>
+ <string>0</string>
+ </array>
+ <key>NSRightMargin</key>
+ <array>
+ <string>float</string>
+ <string>18</string>
+ </array>
+ <key>NSTopMargin</key>
+ <array>
+ <string>float</string>
+ <string>18</string>
+ </array>
+ </dict>
+ <key>ReadOnly</key>
+ <string>NO</string>
+ <key>Sheets</key>
+ <array>
+ <dict>
+ <key>ActiveLayerIndex</key>
+ <integer>0</integer>
+ <key>AutoAdjust</key>
+ <true/>
+ <key>BackgroundGraphic</key>
+ <dict>
+ <key>Bounds</key>
+ <string>{{0, 0}, {806, 1132}}</string>
+ <key>Class</key>
+ <string>SolidGraphic</string>
+ <key>ID</key>
+ <integer>2</integer>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>BaseZoom</key>
+ <integer>0</integer>
+ <key>CanvasOrigin</key>
+ <string>{0, 0}</string>
+ <key>ColumnAlign</key>
+ <integer>1</integer>
+ <key>ColumnSpacing</key>
+ <real>36</real>
+ <key>DisplayScale</key>
+ <string>1 0/72 in = 1.0000 in</string>
+ <key>GraphicsList</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{320.41170773935767, 646.55758982070449}, {100, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>w</key>
+ <string>0</string>
+ </dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>46</integer>
+ <key>Line</key>
+ <dict>
+ <key>ID</key>
+ <integer>45</integer>
+ <key>Position</key>
+ <real>0.49247664213180542</real>
+ <key>RotationType</key>
+ <integer>0</integer>
+ </dict>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 limited / checked}</string>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>44</integer>
+ </dict>
+ <key>ID</key>
+ <integer>45</integer>
+ <key>Points</key>
+ <array>
+ <string>{283.47949897905681, 658.66217570036315}</string>
+ <string>{459.99996984645378, 658.44980851233595}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>1</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>29</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{460.49996948242188, 640.30001831054688}, {248.00003051757812, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>44</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\i\fs24 \cf0 ACTUATOR_CONTROLS_EFFECTIVE_0}</string>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{324, 353.1875}, {120, 28}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>ID</key>
+ <integer>43</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Align</key>
+ <integer>0</integer>
+ <key>Pad</key>
+ <integer>0</integer>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;\red68\green147\blue53;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural
+
+\f0\fs24 \cf2 MAVLink:\
+'MANUAL_CONTROL'}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{324, 174.5}, {129, 28}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>ID</key>
+ <integer>42</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Align</key>
+ <integer>0</integer>
+ <key>Pad</key>
+ <integer>0</integer>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;\red68\green147\blue53;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural
+
+\f0\fs24 \cf2 MAVLink:\
+'RC_CHANNELS_RAW'}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{114.97958374023438, 786}, {191, 14}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>ID</key>
+ <integer>40</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Pad</key>
+ <integer>0</integer>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;\red68\green147\blue53;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf2 MAVLink: 'SERVO_OUTPUT_RAW'}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{528.5, 689}, {112, 14}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>ID</key>
+ <integer>39</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Pad</key>
+ <integer>0</integer>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;\red68\green147\blue53;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf2 MAVLink: 'ctrl eff 0-3'}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>11</integer>
+ </dict>
+ <key>ID</key>
+ <integer>38</integer>
+ <key>Points</key>
+ <array>
+ <string>{584.52245687751122, 125.49999965650382}</string>
+ <string>{584.56069426576869, 157.00000090441219}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>1</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>37</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{448.5, 89}, {272, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>37</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\i\fs24 \cf0 VEHICLE_GLOBAL_POSITION_SETPOINT /\
+VEHICLE_LOCAL_POSITION_SETPOINT}</string>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>27</integer>
+ </dict>
+ <key>ID</key>
+ <integer>34</integer>
+ <key>Points</key>
+ <array>
+ <string>{210.47275259888471, 495.87499981747118}</string>
+ <string>{210.43193555768113, 543.06250166479344}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>1</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>10</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{108, 737.9375}, {205, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>33</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\i\fs24 \cf0 ACTUATOR_OUTPUTS}</string>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>29</integer>
+ </dict>
+ <key>ID</key>
+ <integer>32</integer>
+ <key>Points</key>
+ <array>
+ <string>{210.46968123779743, 580.06249992316259}</string>
+ <string>{210.44442316539605, 627.25000030102035}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>1</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>27</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>33</integer>
+ </dict>
+ <key>ID</key>
+ <integer>30</integer>
+ <key>Points</key>
+ <array>
+ <string>{210.48620562018775, 690.25}</string>
+ <string>{210.49612530146715, 737.4375}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>1</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>29</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{137.97958374023438, 627.75}, {145, 62}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>29</integer>
+ <key>Shape</key>
+ <string>RoundRect</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>b</key>
+ <string>1</string>
+ <key>g</key>
+ <string>0.814996</string>
+ <key>r</key>
+ <string>0.627106</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 Actuator Mixer}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{107.97958374023438, 543.5625}, {205, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>27</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\i\fs24 \cf0 ACTUATOR_CONTROLS_0}</string>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{394.22506405270201, 452.15000309396282}, {89, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>w</key>
+ <string>0</string>
+ </dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>25</integer>
+ <key>Line</key>
+ <dict>
+ <key>ID</key>
+ <integer>22</integer>
+ <key>Position</key>
+ <real>0.5900501012802124</real>
+ <key>RotationType</key>
+ <integer>0</integer>
+ </dict>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 if in auto mode}</string>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{152.49994329565658, 397.00120984204113}, {116, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>w</key>
+ <string>0</string>
+ </dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>24</integer>
+ <key>Line</key>
+ <dict>
+ <key>ID</key>
+ <integer>17</integer>
+ <key>Position</key>
+ <real>0.49406537413597107</real>
+ <key>RotationType</key>
+ <integer>0</integer>
+ </dict>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 if in stabilized mode}</string>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>21</integer>
+ </dict>
+ <key>ID</key>
+ <integer>23</integer>
+ <key>OrthogonalBarAutomatic</key>
+ <true/>
+ <key>OrthogonalBarPoint</key>
+ <string>{0, 0}</string>
+ <key>OrthogonalBarPosition</key>
+ <real>-1</real>
+ <key>Points</key>
+ <array>
+ <string>{584.49994992834695, 220}</string>
+ <string>{584.4997453697481, 348.6875000000008}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>2</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>11</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>10</integer>
+ </dict>
+ <key>ID</key>
+ <integer>22</integer>
+ <key>OrthogonalBarAutomatic</key>
+ <true/>
+ <key>OrthogonalBarPoint</key>
+ <string>{0, 0}</string>
+ <key>OrthogonalBarPosition</key>
+ <real>33.206413269042969</real>
+ <key>Points</key>
+ <array>
+ <string>{583.83119320765968, 385.68717359526977}</string>
+ <string>{581, 464}</string>
+ <string>{283.49994000956832, 464.30111342104294}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>2</integer>
+ <key>Pattern</key>
+ <integer>2</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>21</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{482, 349.1875}, {205, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>21</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\i\fs24 \cf0 VEHICLE_ATTITUDE_SETPOINT}</string>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>10</integer>
+ </dict>
+ <key>ID</key>
+ <integer>17</integer>
+ <key>Points</key>
+ <array>
+ <string>{210.49997491180866, 385.6875}</string>
+ <string>{210.49991091996927, 432.875}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>1</integer>
+ <key>Pattern</key>
+ <integer>2</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>6</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>5</integer>
+ </dict>
+ <key>ID</key>
+ <integer>16</integer>
+ <key>Points</key>
+ <array>
+ <string>{283.49999999999511, 270.00001907176534}</string>
+ <string>{339.50000000000028, 270.00001907176534}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>1</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>9</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>6</integer>
+ </dict>
+ <key>ID</key>
+ <integer>15</integer>
+ <key>Points</key>
+ <array>
+ <string>{210.49998770563047, 301.5}</string>
+ <string>{210.49998770563047, 348.6875}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>1</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>9</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>9</integer>
+ </dict>
+ <key>ID</key>
+ <integer>13</integer>
+ <key>Points</key>
+ <array>
+ <string>{210.49171354592596, 207}</string>
+ <string>{210.49498558851923, 238.4999999999998}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>1</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>4</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>Head</key>
+ <dict>
+ <key>ID</key>
+ <integer>4</integer>
+ </dict>
+ <key>ID</key>
+ <integer>12</integer>
+ <key>Points</key>
+ <array>
+ <string>{210.49498558851246, 138.5000000000002}</string>
+ <string>{210.48997117702493, 170.00000000001342}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>FilledArrow</string>
+ <key>Legacy</key>
+ <true/>
+ <key>LineType</key>
+ <integer>1</integer>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ <key>Tail</key>
+ <dict>
+ <key>ID</key>
+ <integer>8</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{512, 157.5}, {145, 62}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>11</integer>
+ <key>Shape</key>
+ <string>RoundRect</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>b</key>
+ <string>0.999136</string>
+ <key>g</key>
+ <string>0.808554</string>
+ <key>r</key>
+ <string>0.587078</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 Position Controller}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{138, 433.375}, {145, 62}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>10</integer>
+ <key>Shape</key>
+ <string>RoundRect</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>b</key>
+ <string>1</string>
+ <key>g</key>
+ <string>0.814996</string>
+ <key>r</key>
+ <string>0.627106</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 Attitude Controller}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{138, 239}, {145, 62}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>9</integer>
+ <key>Shape</key>
+ <string>RoundRect</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>b</key>
+ <string>0.366082</string>
+ <key>g</key>
+ <string>0.639788</string>
+ <key>r</key>
+ <string>1</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 RC scaling and function mapping}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{138, 76}, {145, 62}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>8</integer>
+ <key>Shape</key>
+ <string>RoundRect</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>b</key>
+ <string>0.324773</string>
+ <key>g</key>
+ <string>0.632962</string>
+ <key>r</key>
+ <string>0.99252</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 RC Input\
+PX4IO or PX4FMU}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{108, 349.1875}, {205, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>6</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\i\fs24 \cf0 MANUAL_CONTROL_SETPOINT}</string>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{340, 252}, {115, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>5</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\i\fs24 \cf0 RC_CHANNELS}</string>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{108, 170.5}, {204.97958374023438, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FontInfo</key>
+ <dict>
+ <key>Font</key>
+ <string>Helvetica</string>
+ <key>Size</key>
+ <real>12</real>
+ </dict>
+ <key>ID</key>
+ <integer>4</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\i\fs24 \cf0 INPUT_RC}</string>
+ </dict>
+ </dict>
+ </array>
+ <key>GridInfo</key>
+ <dict/>
+ <key>HPages</key>
+ <integer>1</integer>
+ <key>KeepToScale</key>
+ <false/>
+ <key>Layers</key>
+ <array>
+ <dict>
+ <key>Lock</key>
+ <string>NO</string>
+ <key>Name</key>
+ <string>Layer 1</string>
+ <key>Print</key>
+ <string>YES</string>
+ <key>View</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>LayoutInfo</key>
+ <dict>
+ <key>Animate</key>
+ <string>NO</string>
+ <key>circoMinDist</key>
+ <real>18</real>
+ <key>circoSeparation</key>
+ <real>0.0</real>
+ <key>layoutEngine</key>
+ <string>dot</string>
+ <key>neatoSeparation</key>
+ <real>0.0</real>
+ <key>twopiSeparation</key>
+ <real>0.0</real>
+ </dict>
+ <key>Orientation</key>
+ <integer>2</integer>
+ <key>PrintOnePage</key>
+ <false/>
+ <key>RowAlign</key>
+ <integer>1</integer>
+ <key>RowSpacing</key>
+ <real>36</real>
+ <key>SheetTitle</key>
+ <string>Canvas 1</string>
+ <key>UniqueID</key>
+ <integer>1</integer>
+ <key>VPages</key>
+ <integer>1</integer>
+ </dict>
+ <dict>
+ <key>ActiveLayerIndex</key>
+ <integer>0</integer>
+ <key>AutoAdjust</key>
+ <true/>
+ <key>BackgroundGraphic</key>
+ <dict>
+ <key>Bounds</key>
+ <string>{{0, 0}, {806, 1132}}</string>
+ <key>Class</key>
+ <string>SolidGraphic</string>
+ <key>ID</key>
+ <integer>2</integer>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>BaseZoom</key>
+ <integer>0</integer>
+ <key>CanvasOrigin</key>
+ <string>{0, 0}</string>
+ <key>ColumnAlign</key>
+ <integer>1</integer>
+ <key>ColumnSpacing</key>
+ <real>36</real>
+ <key>DisplayScale</key>
+ <string>1 0/72 in = 1.0000 in</string>
+ <key>GraphicsList</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{129, 170}, {83, 44}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>4</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ </dict>
+ </array>
+ <key>GridInfo</key>
+ <dict/>
+ <key>HPages</key>
+ <integer>1</integer>
+ <key>KeepToScale</key>
+ <false/>
+ <key>Layers</key>
+ <array>
+ <dict>
+ <key>Lock</key>
+ <string>NO</string>
+ <key>Name</key>
+ <string>Layer 1</string>
+ <key>Print</key>
+ <string>YES</string>
+ <key>View</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>LayoutInfo</key>
+ <dict>
+ <key>Animate</key>
+ <string>NO</string>
+ <key>circoMinDist</key>
+ <real>18</real>
+ <key>circoSeparation</key>
+ <real>0.0</real>
+ <key>layoutEngine</key>
+ <string>dot</string>
+ <key>neatoSeparation</key>
+ <real>0.0</real>
+ <key>twopiSeparation</key>
+ <real>0.0</real>
+ </dict>
+ <key>Orientation</key>
+ <integer>2</integer>
+ <key>PrintOnePage</key>
+ <false/>
+ <key>RowAlign</key>
+ <integer>1</integer>
+ <key>RowSpacing</key>
+ <real>36</real>
+ <key>SheetTitle</key>
+ <string>Remote Control</string>
+ <key>UniqueID</key>
+ <integer>5</integer>
+ <key>VPages</key>
+ <integer>1</integer>
+ </dict>
+ <dict>
+ <key>ActiveLayerIndex</key>
+ <integer>0</integer>
+ <key>AutoAdjust</key>
+ <true/>
+ <key>BackgroundGraphic</key>
+ <dict>
+ <key>Bounds</key>
+ <string>{{0, 0}, {806, 1132}}</string>
+ <key>Class</key>
+ <string>SolidGraphic</string>
+ <key>ID</key>
+ <integer>2</integer>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>BaseZoom</key>
+ <integer>0</integer>
+ <key>CanvasOrigin</key>
+ <string>{0, 0}</string>
+ <key>ColumnAlign</key>
+ <integer>1</integer>
+ <key>ColumnSpacing</key>
+ <real>36</real>
+ <key>DisplayScale</key>
+ <string>1 0/72 in = 1.0000 in</string>
+ <key>GraphicsList</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{575.98995971679688, 379}, {47, 14}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>ID</key>
+ <integer>1124</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Pad</key>
+ <integer>0</integer>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\b\fs24 \cf0 QUAD
+\b0 X}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{576.13064904528869, 216.6250047923653}, {50.666667938232422, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1126</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>b</key>
+ <string>0.13195</string>
+ <key>g</key>
+ <string>0.165669</string>
+ <key>r</key>
+ <string>1</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{565.46398168220912, 200.5990128790099}, {72, 72}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1127</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1125</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{684.97877057518497, 320.1208883952101}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1129</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 4}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1131</integer>
+ <key>Points</key>
+ <array>
+ <string>{744.59415627118847, 308.1935744741462}</string>
+ <string>{742.38444757998036, 309.34262299357425}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{656.69108999710829, 361.8760779997491}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1132</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{646.29122523639319, 281.66198172978545}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1133</integer>
+ <key>Rotation</key>
+ <real>135</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{643.54122523639319, 278.91198172978545}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1134</integer>
+ <key>Rotation</key>
+ <real>135</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1130</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1136</integer>
+ <key>Points</key>
+ <array>
+ <string>{649.54722336322868, 356.21791362626482}</string>
+ <string>{651.75693205443667, 355.0688651068366}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{721.45028963730863, 284.03541010066203}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1137</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{646.35015439802385, 281.24950637062551}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1138</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{643.60015439802385, 278.49950637062551}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1139</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1135</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{646.0670873832953, 281.20919655324775}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1140</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1128</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{494.01518801866729, 320.05486147948341}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1142</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 2}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1144</integer>
+ <key>Points</key>
+ <array>
+ <string>{458.75335031844725, 307.77399956263616}</string>
+ <string>{460.96305900965541, 308.92304808206421}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{530.65641659252742, 361.45650308823895}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1145</integer>
+ <key>Rotation</key>
+ <real>135</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{455.55628135324292, 281.24240681827519}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1146</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{452.80628135324292, 278.49240681827519}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1147</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1143</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1149</integer>
+ <key>Points</key>
+ <array>
+ <string>{553.44673702887792, 356.15188491228344}</string>
+ <string>{551.23702833767004, 355.00283639285527}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{465.54367075479814, 283.96938138668088}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1150</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{455.14380599408253, 281.1834776566443}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1151</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{452.39380599408253, 278.4334776566443}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1152</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1148</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{455.10349617670488, 281.1431681805102}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1153</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1141</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{684.74305392865585, 129.32699556949493}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1155</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 1}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1157</integer>
+ <key>Points</key>
+ <array>
+ <string>{649.48121622843564, 117.04613365264774}</string>
+ <string>{651.69092491964352, 118.19518217207589}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{721.38428250251582, 170.72863717825052}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1158</integer>
+ <key>Rotation</key>
+ <real>135</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{646.28414726323126, 90.514540908286847}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1159</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{643.53414726323126, 87.764540908286847}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1160</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1156</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1162</integer>
+ <key>Points</key>
+ <array>
+ <string>{744.17460293886643, 165.42401900229501}</string>
+ <string>{741.96489424765866, 164.27497048286693}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{656.27153666478659, 93.241515476692427}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1163</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{645.87167190407115, 90.455611746655876}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1164</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{643.12167190407115, 87.705611746655876}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1165</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1161</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{645.83136208669339, 90.415302270521892}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1166</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1154</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{494.00811680064464, 129.15022794494948}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1168</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 3}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1170</integer>
+ <key>Points</key>
+ <array>
+ <string>{553.62350249664826, 117.22291402388569}</string>
+ <string>{551.41379380544015, 118.37196254331369}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{465.72043622256808, 170.9054175494887}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1171</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{455.32057146185286, 90.691321279524772}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1172</integer>
+ <key>Rotation</key>
+ <real>135</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{452.57057146185286, 87.941321279524772}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1173</integer>
+ <key>Rotation</key>
+ <real>135</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1169</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1175</integer>
+ <key>Points</key>
+ <array>
+ <string>{458.57656958868847, 165.24725317600405}</string>
+ <string>{460.78627827989641, 164.09820465657594}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{530.47963586276819, 93.064749650401353}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1176</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{455.37950062348352, 90.278845920365001}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1177</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{452.62950062348352, 87.528845920365001}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1178</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1174</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{455.09643360875504, 90.238536102987126}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1179</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1167</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{547.25939156789127, 219.8035943432547}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1180</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{647.66855809291212, 119.39442062517122}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1181</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{647.66855089985006, 219.80357636059938}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1182</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{547.20046240626141, 119.33550225313491}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1183</integer>
+ <key>Rotation</key>
+ <real>315</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1123</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{287.14883422851562, 373}, {46, 14}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>ID</key>
+ <integer>1121</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Pad</key>
+ <integer>0</integer>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\b\fs24 \cf0 QUAD
+\b0 +}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{252.93672553699014, 864.96332065264312}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1186</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 6}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1188</integer>
+ <key>Points</key>
+ <array>
+ <string>{248.68672172228349, 826.3749926090228}</string>
+ <string>{249.43672172228361, 828.7499926090228}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{252.68672172228372, 918.12499260902291}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1189</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{214.43672172228372, 826.3749926090228}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1190</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{211.68672172228372, 823.6249926090228}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1191</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1187</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1193</integer>
+ <key>Points</key>
+ <array>
+ <string>{281.43672680854809, 927.54165927568897}</string>
+ <string>{280.6867268085482, 925.1666592756892}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{261.43672680854797, 817.2916592756892}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1194</integer>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{214.18672680854797, 826.0416592756892}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1195</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{211.43672680854797, 823.2916592756892}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1196</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1192</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{214.02503832182103, 826.05162629068673}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1197</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1185</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{255.93671290080545, 636.38001505533919}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1199</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 5}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1201</integer>
+ <key>Points</key>
+ <array>
+ <string>{251.6867090860988, 597.79168701171886}</string>
+ <string>{252.43670908609892, 600.16668701171886}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{255.68670908609903, 689.54168701171898}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1202</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{217.43670908609903, 597.79168701171886}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1203</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{214.68670908609903, 595.04168701171886}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1204</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1200</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1206</integer>
+ <key>Points</key>
+ <array>
+ <string>{284.43671417236339, 698.95835367838504}</string>
+ <string>{283.68671417236351, 696.58335367838527}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{264.43671417236328, 588.70835367838527}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1207</integer>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{217.18671417236328, 597.45835367838527}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1208</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{214.43671417236328, 594.70835367838527}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1209</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1205</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{217.02502568563636, 597.46832069338279}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1210</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1198</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{480.49999872844216, 864.96333555380545}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1212</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 4}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1214</integer>
+ <key>Points</key>
+ <array>
+ <string>{476.24999491373552, 826.37500751018513}</string>
+ <string>{476.99999491373563, 828.75000751018513}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{480.24999491373575, 918.12500751018524}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1215</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{441.99999491373575, 826.37500751018513}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1216</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{439.24999491373575, 823.62500751018513}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1217</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1213</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1219</integer>
+ <key>Points</key>
+ <array>
+ <string>{509.00000000000011, 927.5416741768513}</string>
+ <string>{508.25000000000023, 925.16667417685153}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{489, 817.29167417685153}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1220</integer>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{441.75, 826.04167417685153}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1221</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{439, 823.29167417685153}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1222</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1218</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{441.58831151327308, 826.05164119184906}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1223</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1211</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{480.49999872844216, 636.38001505533896}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1225</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 3}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1227</integer>
+ <key>Points</key>
+ <array>
+ <string>{476.24999491373552, 597.79168701171864}</string>
+ <string>{476.99999491373563, 600.16668701171864}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{480.24999491373575, 689.54168701171875}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1228</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{441.99999491373575, 597.79168701171864}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1229</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{439.24999491373575, 595.04168701171864}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1230</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1226</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1232</integer>
+ <key>Points</key>
+ <array>
+ <string>{509.00000000000011, 698.95835367838481}</string>
+ <string>{508.25000000000023, 696.58335367838504}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{489, 588.70835367838504}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1233</integer>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{441.75, 597.45835367838504}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1234</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{439, 594.70835367838504}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1235</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1231</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{441.58831151327308, 597.46832069338257}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1236</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1224</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{452.7625732421875, 947.72591400146473}, {45, 14}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>ID</key>
+ <integer>1237</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Pad</key>
+ <integer>0</integer>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\b\fs24 \cf0 OCTO
+\b0 +}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{353.51001119036124, 739.42163658141953}, {50.666667938232422, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1239</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>b</key>
+ <string>0.13195</string>
+ <key>g</key>
+ <string>0.165669</string>
+ <key>r</key>
+ <string>1</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{342.8433593880041, 726.66665752440122}, {72, 72}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1240</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1238</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{366.83836873372331, 916.58333714803678}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1242</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 2}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1244</integer>
+ <key>Points</key>
+ <array>
+ <string>{429.42669677734398, 945.3333333333303}</string>
+ <string>{427.05169677734398, 944.58333333333019}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{320.42669677734375, 924.08333333333019}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1245</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{327.92669677734398, 878.08333333333019}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1246</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{325.17669677734398, 875.33333333333019}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1247</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1243</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1249</integer>
+ <key>Points</key>
+ <array>
+ <string>{328.26003011067723, 912.08333841959472}</string>
+ <string>{330.63503011067723, 912.83333841959472}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{421.26003011067723, 914.83333841959472}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1250</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{328.26003011067723, 877.83333841959472}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1251</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{325.51003011067723, 875.08333841959472}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1252</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1248</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{327.92668660481695, 877.67164993286758}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1253</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1241</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{205.66167195637968, 750.54168065389615}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1255</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 7}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1257</integer>
+ <key>Points</key>
+ <array>
+ <string>{268.25000000000023, 779.29167683918968}</string>
+ <string>{265.87500000000023, 778.54167683918956}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{159.24999999999994, 758.04167683918956}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1258</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{166.75, 712.04167683918956}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1259</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{164, 709.29167683918956}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1260</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1256</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1262</integer>
+ <key>Points</key>
+ <array>
+ <string>{167.08333333333348, 746.0416819254541}</string>
+ <string>{169.45833333333348, 746.7916819254541}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{260.08333333333348, 748.7916819254541}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1263</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{167.08333333333348, 711.7916819254541}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1264</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{164.33333333333348, 709.0416819254541}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1265</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1261</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{166.74998982747337, 711.62999343872696}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1266</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1254</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{528.01506551106706, 750.54168065389615}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1268</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 8}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1270</integer>
+ <key>Points</key>
+ <array>
+ <string>{590.60339355468773, 779.29167683918968}</string>
+ <string>{588.22839355468773, 778.54167683918956}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{481.6033935546875, 758.04167683918956}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1271</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{489.10339355468773, 712.04167683918956}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1272</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{486.35339355468773, 709.29167683918956}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1273</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1269</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1275</integer>
+ <key>Points</key>
+ <array>
+ <string>{489.43672688802098, 746.0416819254541}</string>
+ <string>{491.81172688802098, 746.7916819254541}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{582.43672688802098, 748.7916819254541}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1276</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{489.43672688802098, 711.7916819254541}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1277</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{486.68672688802098, 709.0416819254541}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1278</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1274</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{489.10338338216064, 711.62999343872696}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1279</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1267</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{306.14882584901056, 766.45459545146662}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1280</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{468.3533935546875, 695.66666666666504}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1281</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{440.55795213074884, 760.88380015384382}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1282</integer>
+ <key>Rotation</key>
+ <real>135</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{366.83834838867119, 584.49999872844194}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1284</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 1}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1286</integer>
+ <key>Points</key>
+ <array>
+ <string>{429.42667643229174, 613.24999491373558}</string>
+ <string>{427.05167643229174, 612.49999491373546}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{320.42667643229152, 591.99999491373546}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1287</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{327.92667643229174, 545.99999491373546}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1288</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{325.17667643229174, 543.24999491373546}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1289</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1285</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1291</integer>
+ <key>Points</key>
+ <array>
+ <string>{328.26000976562511, 579.99999999999977}</string>
+ <string>{330.63500976562511, 580.74999999999977}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{421.260009765625, 582.74999999999977}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1292</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{328.26000976562523, 545.74999999999977}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1293</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{325.51000976562523, 542.99999999999977}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1294</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1290</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{327.92666625976494, 545.58831151327263}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1295</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1283</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{281.3533935546875, 695.66666666666504}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1296</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{442.5579845556756, 629.04542696963983}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1297</integer>
+ <key>Rotation</key>
+ <real>225</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{374.84336344400776, 790.58333841959472}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1298</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{374.84335708617891, 600.24999491373535}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1299</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{309.12873002381525, 629.45457637798029}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1300</integer>
+ <key>Rotation</key>
+ <real>135</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1184</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{181.15661763567644, 222.11968549092626}, {50.666667938232422, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1024</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>b</key>
+ <string>0.13195</string>
+ <key>g</key>
+ <string>0.165669</string>
+ <key>r</key>
+ <string>1</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{170.4899658333193, 209.364706433908}, {72, 72}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1025</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1023</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{194.48495483398159, 368.44802729289523}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1027</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 4}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1029</integer>
+ <key>Points</key>
+ <array>
+ <string>{257.0732828776022, 397.19802347818882}</string>
+ <string>{254.69828287760208, 396.4480234781887}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{148.07328287760188, 375.9480234781887}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1030</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{155.57328287760205, 329.9480234781887}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1031</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{152.82328287760205, 327.1980234781887}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1032</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1028</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1034</integer>
+ <key>Points</key>
+ <array>
+ <string>{155.90661621093548, 363.94802856445312}</string>
+ <string>{158.28161621093548, 364.69802856445312}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{248.90661621093545, 366.69802856445312}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1035</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{155.90661621093548, 329.69802856445312}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1036</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{153.15661621093548, 326.94802856445312}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1037</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1033</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{155.57327270507534, 329.53634007772598}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1038</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1026</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{59.499998728441994, 233.36969502765339}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1040</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 2}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1042</integer>
+ <key>Points</key>
+ <array>
+ <string>{55.249994913735321, 194.78136698403296}</string>
+ <string>{55.999994913735435, 197.1563669840329}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{59.249994913735435, 286.53136698403307}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1043</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{20.999994913735549, 194.78136698403301}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1044</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{18.249994913735549, 192.03136698403301}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1045</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1041</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1047</integer>
+ <key>Points</key>
+ <array>
+ <string>{87.999999999999915, 295.9480336506993}</string>
+ <string>{87.250000000000028, 293.57303365069941}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{67.999999999999915, 185.69803365069959}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1048</integer>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{20.749999999999915, 194.44803365069947}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1049</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{17.999999999999915, 191.69803365069947}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1050</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1046</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{20.588311513272686, 194.45800066569694}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1051</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1039</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{329.22993342082486, 233.36969502765339}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1053</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 1}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1055</integer>
+ <key>Points</key>
+ <array>
+ <string>{324.97992960611816, 194.7813669840329}</string>
+ <string>{325.72992960611828, 197.15636698403287}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{328.97992960611828, 286.53136698403307}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1056</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{290.72992960611839, 194.78136698403301}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1057</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{287.97992960611839, 192.03136698403301}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1058</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1054</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1060</integer>
+ <key>Points</key>
+ <array>
+ <string>{357.72993469238281, 295.9480336506993}</string>
+ <string>{356.97993469238293, 293.57303365069941}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{337.72993469238281, 185.69803365069953}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1061</integer>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{290.47993469238281, 194.44803365069947}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1062</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{287.72993469238281, 191.69803365069947}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1063</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1059</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{290.31824620565556, 194.45800066569686}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1064</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1052</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{194.48495483398159, 92.448027292895233}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1066</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 3}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1068</integer>
+ <key>Points</key>
+ <array>
+ <string>{257.0732828776022, 121.19802347818882}</string>
+ <string>{254.69828287760208, 120.4480234781887}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{148.07328287760188, 99.948023478188702}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1069</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{155.57328287760205, 53.948023478188688}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1070</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{152.82328287760205, 51.198023478188688}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1071</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1067</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1073</integer>
+ <key>Points</key>
+ <array>
+ <string>{155.90661621093548, 87.948028564453111}</string>
+ <string>{158.28161621093548, 88.698028564453111}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{248.90661621093545, 90.698028564453111}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1074</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{155.90661621093548, 53.698028564453111}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1075</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{153.15661621093548, 50.948028564453111}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1076</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1072</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{155.57327270507534, 53.536340077725967}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1077</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1065</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{131.48995971679412, 178.36470031738281}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1078</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{273.48996988932043, 178.36469523111947}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1079</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{202.48996988932029, 249.36468505859375}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1080</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{202.48995971679412, 107.28136189778644}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1081</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1022</integer>
+ </dict>
+ </array>
+ <key>GridInfo</key>
+ <dict/>
+ <key>HPages</key>
+ <integer>1</integer>
+ <key>KeepToScale</key>
+ <false/>
+ <key>Layers</key>
+ <array>
+ <dict>
+ <key>Lock</key>
+ <string>NO</string>
+ <key>Name</key>
+ <string>Layer 1</string>
+ <key>Print</key>
+ <string>YES</string>
+ <key>View</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>LayoutInfo</key>
+ <dict>
+ <key>Animate</key>
+ <string>NO</string>
+ <key>circoMinDist</key>
+ <real>18</real>
+ <key>circoSeparation</key>
+ <real>0.0</real>
+ <key>layoutEngine</key>
+ <string>dot</string>
+ <key>neatoSeparation</key>
+ <real>0.0</real>
+ <key>twopiSeparation</key>
+ <real>0.0</real>
+ </dict>
+ <key>Orientation</key>
+ <integer>2</integer>
+ <key>PrintOnePage</key>
+ <false/>
+ <key>RowAlign</key>
+ <integer>1</integer>
+ <key>RowSpacing</key>
+ <real>36</real>
+ <key>SheetTitle</key>
+ <string>Rotor Assignments</string>
+ <key>UniqueID</key>
+ <integer>2</integer>
+ <key>VPages</key>
+ <integer>1</integer>
+ </dict>
+ <dict>
+ <key>ActiveLayerIndex</key>
+ <integer>0</integer>
+ <key>AutoAdjust</key>
+ <true/>
+ <key>BackgroundGraphic</key>
+ <dict>
+ <key>Bounds</key>
+ <string>{{0, 0}, {806, 1132}}</string>
+ <key>Class</key>
+ <string>SolidGraphic</string>
+ <key>ID</key>
+ <integer>2</integer>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>BaseZoom</key>
+ <integer>0</integer>
+ <key>CanvasOrigin</key>
+ <string>{0, 0}</string>
+ <key>ColumnAlign</key>
+ <integer>1</integer>
+ <key>ColumnSpacing</key>
+ <real>36</real>
+ <key>DisplayScale</key>
+ <string>1 0/72 in = 1.0000 in</string>
+ <key>GraphicsList</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{479, 926}, {45, 14}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>ID</key>
+ <integer>1210</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Pad</key>
+ <integer>0</integer>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\b\fs24 \cf0 HEXA
+\b0 X}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{323.13577025880016, 749.76557170439401}, {50.666667938232422, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1124</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>b</key>
+ <string>0.13195</string>
+ <key>g</key>
+ <string>0.165669</string>
+ <key>r</key>
+ <string>1</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{312.46911845644303, 737.01059264737569}, {72, 72}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1125</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1123</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{407.17230315305846, 883.72204834096681}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1127</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 4}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1129</integer>
+ <key>Points</key>
+ <array>
+ <string>{379.80522301611819, 860.03627302756399}</string>
+ <string>{381.64224206895665, 861.71808336155198}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{432.69752786153174, 932.25483880979243}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1130</integer>
+ <key>Rotation</key>
+ <real>150</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{368.7196421785618, 844.9870622696244}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1131</integer>
+ <key>Rotation</key>
+ <real>60</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{365.9696421785618, 842.2370622696244}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1132</integer>
+ <key>Rotation</key>
+ <real>60</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1128</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1134</integer>
+ <key>Points</key>
+ <array>
+ <string>{458.75089272822612, 931.27417383395721}</string>
+ <string>{456.91387367538789, 929.59236349996911}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{389.85858788281303, 840.55560805172911}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1135</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{368.33647356578285, 844.82338459189691}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1136</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{365.58647356578285, 842.07338459189691}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1137</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1133</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{368.26061274159781, 844.81035428333826}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1138</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1126</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{267.75726457856609, 881.27816449485533}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1140</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 6}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1142</integer>
+ <key>Points</key>
+ <array>
+ <string>{331.94303989196902, 882.48992268280176}</string>
+ <string>{329.51122955798087, 883.02790362996325}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{230.47447410974044, 913.3476178373885}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1143</integer>
+ <key>Rotation</key>
+ <real>60</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{228.99225064990861, 842.82550352035832}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1144</integer>
+ <key>Rotation</key>
+ <real>150</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{226.24225064990861, 840.07550352035832}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1145</integer>
+ <key>Rotation</key>
+ <real>150</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1141</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1147</integer>
+ <key>Points</key>
+ <array>
+ <string>{227.70513908557558, 904.27791574513662}</string>
+ <string>{230.13694941956365, 903.73993479797491}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{313.17370486780385, 854.92022059054989}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1148</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{229.15592832763582, 842.44233490757972}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1149</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{226.40592832763582, 839.69233490757972}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1150</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1146</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{228.84558214533197, 842.36647408339456}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1151</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1139</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{474.97119637474935, 761.64314519248808}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1153</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 1}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1155</integer>
+ <key>Points</key>
+ <array>
+ <string>{539.15697168815223, 762.85490338043462}</string>
+ <string>{536.72516135416413, 763.39288432759611}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{437.68840590592367, 793.71259853502136}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1156</integer>
+ <key>Rotation</key>
+ <real>60</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{436.20618244609187, 723.19048421799118}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1157</integer>
+ <key>Rotation</key>
+ <real>150</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{433.45618244609187, 720.44048421799118}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1158</integer>
+ <key>Rotation</key>
+ <real>150</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1154</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1160</integer>
+ <key>Points</key>
+ <array>
+ <string>{434.91907088175884, 784.64289644276937}</string>
+ <string>{437.35088121574688, 784.10491549560766}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{520.38763666398711, 735.28520128818252}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1161</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{436.36986012381902, 722.80731560521247}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1162</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{433.61986012381902, 720.05731560521247}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1163</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1159</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{436.05951394151521, 722.73145478102731}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1164</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1152</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{197.82245308809811, 760.38763453076365}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1166</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 2}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1168</integer>
+ <key>Points</key>
+ <array>
+ <string>{170.45537295115798, 736.70185921736106}</string>
+ <string>{172.29239200399638, 738.38366955134893}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{223.34767779657142, 808.92042499958939}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1169</integer>
+ <key>Rotation</key>
+ <real>150</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{159.36979211360145, 721.65264845942124}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1170</integer>
+ <key>Rotation</key>
+ <real>60</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{156.61979211360145, 718.90264845942124}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1171</integer>
+ <key>Rotation</key>
+ <real>60</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1167</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1173</integer>
+ <key>Points</key>
+ <array>
+ <string>{249.4010426632658, 807.93976002375393}</string>
+ <string>{247.56402361042768, 806.25794968976606}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{180.50873781785265, 717.22119424152584}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1174</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{158.98662350082267, 721.48897078169387}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1175</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{156.23662350082267, 718.73897078169387}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1176</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1172</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{158.91076267663743, 721.47594047313521}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1177</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1165</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{405.7864385681853, 642.051622339657}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1179</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 5}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1181</integer>
+ <key>Points</key>
+ <array>
+ <string>{378.41935843124503, 618.36584702625407}</string>
+ <string>{380.25637748408349, 620.04765736024217}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{431.31166327665852, 690.58441280848263}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1182</integer>
+ <key>Rotation</key>
+ <real>150</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{367.33377759368858, 603.3166362683146}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1183</integer>
+ <key>Rotation</key>
+ <real>60</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{364.58377759368858, 600.5666362683146}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1184</integer>
+ <key>Rotation</key>
+ <real>60</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1180</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1186</integer>
+ <key>Points</key>
+ <array>
+ <string>{457.36502814335307, 689.6037478326474}</string>
+ <string>{455.52800909051496, 687.92193749865953}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{388.47272329793987, 598.8851820504193}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1187</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{366.95060898090981, 603.15295859058722}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1188</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{364.20060898090981, 600.40295859058722}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1189</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1185</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{366.87474815672459, 603.13992828202845}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1190</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1178</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{416.05401025695778, 706.97194681357246}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1191</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{273.20840928884093, 706.24568349439642}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1192</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{309.23428435411637, 768.64435011115415}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1193</integer>
+ <key>Rotation</key>
+ <real>30</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{380.02813770004576, 644.57328454142464}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1194</integer>
+ <key>Rotation</key>
+ <real>30</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{268.61421230672084, 643.49244485638758}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1196</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 3}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1198</integer>
+ <key>Points</key>
+ <array>
+ <string>{332.79998762012377, 644.70420304433412}</string>
+ <string>{330.36817728613562, 645.24218399149561}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{231.33142183789516, 675.56189819892086}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1199</integer>
+ <key>Rotation</key>
+ <real>60</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{229.84919837806336, 605.03978388189068}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1200</integer>
+ <key>Rotation</key>
+ <real>150</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{227.09919837806336, 602.28978388189068}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1201</integer>
+ <key>Rotation</key>
+ <real>150</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1197</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1203</integer>
+ <key>Points</key>
+ <array>
+ <string>{228.56208681373033, 666.49219610666887}</string>
+ <string>{230.99389714771837, 665.95421515950716}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{314.03065259595854, 617.13450095208213}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1204</integer>
+ <key>Rotation</key>
+ <real>240</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{230.01287605579051, 604.65661526911197}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1205</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{227.26287605579051, 601.90661526911197}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1206</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1202</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{229.7025298734867, 604.58075444492681}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1207</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1195</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{380.57688435489479, 768.55104482376805}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1208</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{309.53521396482506, 645.50328259865807}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1209</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1122</integer>
+ <key>Rotation</key>
+ <real>330</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{431, 450}, {44, 14}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>ID</key>
+ <integer>1121</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Pad</key>
+ <integer>0</integer>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\b\fs24 \cf0 HEXA
+\b0 +}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{326.15660523791308, 258.95613225301122}, {50.666667938232422, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>320</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>b</key>
+ <string>0.13195</string>
+ <key>g</key>
+ <string>0.165669</string>
+ <key>r</key>
+ <string>1</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{315.48995343555595, 246.20115319599296}, {72, 72}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>321</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>319</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{339.36497966449247, 411.82398351031941}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>323</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 2}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>325</integer>
+ <key>Points</key>
+ <array>
+ <string>{335.11497584978588, 373.23565546669897}</string>
+ <string>{335.86497584978599, 375.61065546669897}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{339.11497584978599, 464.98565546669914}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>326</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{300.86497584978611, 373.23565546669909}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>327</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{298.11497584978611, 370.48565546669909}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>328</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>324</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>330</integer>
+ <key>Points</key>
+ <array>
+ <string>{367.86498093605042, 474.40232213336532}</string>
+ <string>{367.11498093605053, 472.02732213336543}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{347.86498093605042, 364.15232213336554}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>331</integer>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{300.61498093605042, 372.90232213336543}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>332</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{297.86498093605042, 370.15232213336543}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>333</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>329</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{300.45329244932327, 372.91228914836296}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>334</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>322</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{219.84995651245046, 339.99999872844211}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>336</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 3}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>338</integer>
+ <key>Points</key>
+ <array>
+ <string>{282.43828455607104, 368.74999491373569}</string>
+ <string>{280.06328455607098, 367.99999491373558}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{173.43828455607076, 347.49999491373558}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>339</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{180.93828455607093, 301.49999491373558}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>340</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{178.18828455607093, 298.74999491373558}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>341</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>337</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>343</integer>
+ <key>Points</key>
+ <array>
+ <string>{181.27161788940435, 335.5}</string>
+ <string>{183.64661788940435, 336.25}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{274.2716178894043, 338.25}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>344</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{181.27161788940435, 301.25}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>345</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{178.52161788940435, 298.5}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>346</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>342</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{180.93827438354421, 301.08831151327286}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>347</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>335</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{459.11999511718477, 339.99999872844211}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>349</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 6}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>351</integer>
+ <key>Points</key>
+ <array>
+ <string>{521.70832316080532, 368.74999491373569}</string>
+ <string>{519.33332316080532, 367.99999491373558}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{412.70832316080504, 347.49999491373558}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>352</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{420.20832316080521, 301.49999491373558}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>353</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{417.45832316080521, 298.74999491373558}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>354</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>350</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>356</integer>
+ <key>Points</key>
+ <array>
+ <string>{420.54165649413864, 335.5}</string>
+ <string>{422.91665649413864, 336.25}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{513.54165649413858, 338.25}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>357</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{420.54165649413864, 301.25}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>358</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{417.79165649413864, 298.5}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>359</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>355</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{420.20831298827852, 301.08831151327286}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>360</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>348</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{219.72989813487507, 200.33832295734095}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>362</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 5}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>364</integer>
+ <key>Points</key>
+ <array>
+ <string>{215.47989432016846, 161.74999491372051}</string>
+ <string>{216.22989432016857, 164.12499491372046}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{219.47989432016857, 253.49999491372063}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>365</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{181.22989432016868, 161.74999491372057}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>366</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{178.47989432016868, 158.99999491372057}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>367</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>363</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>369</integer>
+ <key>Points</key>
+ <array>
+ <string>{248.22989940643305, 262.91666158038686}</string>
+ <string>{247.47989940643316, 260.54166158038697}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{228.22989940643305, 152.66666158038709}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>370</integer>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{180.97989940643305, 161.41666158038697}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>371</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{178.22989940643305, 158.66666158038697}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>372</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>368</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{180.81821091970582, 161.42662859538444}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>373</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>361</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{458.99999872844205, 201.83832295734095}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>375</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 4}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>377</integer>
+ <key>Points</key>
+ <array>
+ <string>{454.74999491373541, 163.24999491372051}</string>
+ <string>{455.49999491373552, 165.62499491372046}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{458.74999491373552, 254.99999491372063}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>378</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{420.49999491373563, 163.24999491372057}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>379</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{417.74999491373563, 160.49999491372057}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>380</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>376</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>382</integer>
+ <key>Points</key>
+ <array>
+ <string>{487.5, 264.41666158038686}</string>
+ <string>{486.75000000000011, 262.04166158038697}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{467.5, 154.16666158038709}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>383</integer>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{420.25, 162.91666158038697}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>384</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{417.5, 160.16666158038697}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>385</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>381</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{420.0883115132728, 162.92662859538444}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>386</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>374</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{409.00361117886365, 251.82615622621091}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>387</integer>
+ <key>Rotation</key>
+ <real>300</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{285.65882358120746, 179.77439325790914}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>388</integer>
+ <key>Rotation</key>
+ <real>300</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{285.65881327292152, 251.82616124293514}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>389</integer>
+ <key>Rotation</key>
+ <real>60</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{409.00362148715112, 179.77439325790914}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>390</integer>
+ <key>Rotation</key>
+ <real>60</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{339.48495483398165, 134.49999872844211}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>392</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 1}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>394</integer>
+ <key>Points</key>
+ <array>
+ <string>{402.0732828776022, 163.24999491373569}</string>
+ <string>{399.69828287760214, 162.49999491373558}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{293.07328287760191, 141.99999491373558}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>395</integer>
+ <key>Rotation</key>
+ <real>90</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{300.57328287760208, 95.999994913735577}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>396</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{297.82328287760208, 93.249994913735577}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>397</integer>
+ <key>Rotation</key>
+ <real>180</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>393</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>399</integer>
+ <key>Points</key>
+ <array>
+ <string>{300.90661621093551, 130}</string>
+ <string>{303.28161621093551, 130.75}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{393.90661621093545, 132.75}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>400</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{300.90661621093551, 95.75}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>401</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{298.15661621093551, 93}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>402</integer>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>398</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{300.5732727050754, 95.588311513272842}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>403</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>391</integer>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{347.48996988932038, 287.41665649414062}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>404</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{347.4899597167942, 145.33333333333331}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>405</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>318</integer>
+ </dict>
+ </array>
+ <key>GridInfo</key>
+ <dict/>
+ <key>HPages</key>
+ <integer>1</integer>
+ <key>KeepToScale</key>
+ <false/>
+ <key>Layers</key>
+ <array>
+ <dict>
+ <key>Lock</key>
+ <string>NO</string>
+ <key>Name</key>
+ <string>Layer 1</string>
+ <key>Print</key>
+ <string>YES</string>
+ <key>View</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>LayoutInfo</key>
+ <dict>
+ <key>Animate</key>
+ <string>NO</string>
+ <key>circoMinDist</key>
+ <real>18</real>
+ <key>circoSeparation</key>
+ <real>0.0</real>
+ <key>layoutEngine</key>
+ <string>dot</string>
+ <key>neatoSeparation</key>
+ <real>0.0</real>
+ <key>twopiSeparation</key>
+ <real>0.0</real>
+ </dict>
+ <key>Orientation</key>
+ <integer>2</integer>
+ <key>PrintOnePage</key>
+ <false/>
+ <key>RowAlign</key>
+ <integer>1</integer>
+ <key>RowSpacing</key>
+ <real>36</real>
+ <key>SheetTitle</key>
+ <string>Rotor Assignment 2</string>
+ <key>UniqueID</key>
+ <integer>3</integer>
+ <key>VPages</key>
+ <integer>1</integer>
+ </dict>
+ <dict>
+ <key>ActiveLayerIndex</key>
+ <integer>0</integer>
+ <key>AutoAdjust</key>
+ <true/>
+ <key>BackgroundGraphic</key>
+ <dict>
+ <key>Bounds</key>
+ <string>{{0, 0}, {806, 1132}}</string>
+ <key>Class</key>
+ <string>SolidGraphic</string>
+ <key>ID</key>
+ <integer>2</integer>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>BaseZoom</key>
+ <integer>0</integer>
+ <key>CanvasOrigin</key>
+ <string>{0, 0}</string>
+ <key>ColumnAlign</key>
+ <integer>1</integer>
+ <key>ColumnSpacing</key>
+ <real>36</real>
+ <key>DisplayScale</key>
+ <string>1 0/72 in = 1.0000 in</string>
+ <key>GraphicsList</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{317.51000800051321, 240.42163882949882}, {50.666667938232422, 36}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1417</integer>
+ <key>Rotation</key>
+ <real>270</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>b</key>
+ <string>0.13195</string>
+ <key>g</key>
+ <string>0.165669</string>
+ <key>r</key>
+ <string>1</string>
+ </dict>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{306.84335619815613, 227.6666597724805}, {72, 72}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1418</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>CornerRadius</key>
+ <real>9</real>
+ </dict>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1416</integer>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{181.87174317649047, 313.76849034719959}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1421</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 6}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1423</integer>
+ <key>Points</key>
+ <array>
+ <string>{159.49938242361688, 285.249576623557}</string>
+ <string>{161.10116522486757, 287.15677793899749}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{201.23696348262672, 364.71920821881605}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1424</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{143.41457890258161, 275.07218626448082}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1425</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{140.66457890258161, 272.32218626448082}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1426</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1422</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1428</integer>
+ <key>Points</key>
+ <array>
+ <string>{228.47124867019181, 366.1825051496383}</string>
+ <string>{226.8694658689414, 364.27530383419804}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{170.73366761118237, 268.21287355437971}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1429</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{143.05605219122737, 274.85989550871483}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1430</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{140.30605219122737, 272.10989550871483}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1431</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1427</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{142.96005354220407, 274.85679605657162}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1432</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1420</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{272.11841406704639, 103.73309829351469}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1434</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 5}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1436</integer>
+ <key>Points</key>
+ <array>
+ <string>{249.74605331417285, 75.214184569872145}</string>
+ <string>{251.34783611542343, 77.121385885312606}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{291.48363437318255, 154.68381616513099}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1437</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{233.66124979313764, 65.036794210795819}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1438</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{230.91124979313764, 62.286794210795819}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1439</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1435</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1441</integer>
+ <key>Points</key>
+ <array>
+ <string>{318.71791956074799, 156.14711309595339}</string>
+ <string>{317.11613675949758, 154.23991178051318}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{260.98033850173846, 58.177481500694711}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1442</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{233.30272308178348, 64.824503455029671}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1443</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{230.55272308178348, 62.074503455029671}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1444</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1440</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{233.20672443276007, 64.821404002886837}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1445</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1433</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{392.11278792691934, 400.85319857921803}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1447</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 4}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1449</integer>
+ <key>Points</key>
+ <array>
+ <string>{369.74042717404592, 372.33428485557562}</string>
+ <string>{371.34220997529656, 374.24148617101616}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{411.47800823305562, 451.80391645083449}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1450</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{353.65562365301071, 362.15689449649926}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1451</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{350.90562365301071, 359.40689449649926}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1452</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1448</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1454</integer>
+ <key>Points</key>
+ <array>
+ <string>{438.71229342062088, 453.26721338165692}</string>
+ <string>{437.11051061937059, 451.36001206621654}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{380.97471236161118, 355.29758178639815}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1455</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{353.29709694165621, 361.94460374073327}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1456</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{350.54709694165621, 359.19460374073327}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1457</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1453</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{353.20109829263305, 361.94150428859024}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1458</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1446</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{479.58783759318999, 189.66974728965576}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1460</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 3}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1462</integer>
+ <key>Points</key>
+ <array>
+ <string>{457.21547665220845, 161.15083366194582}</string>
+ <string>{458.81725945345897, 163.05803497738634}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{498.95305771121809, 240.62046525720473}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1463</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{441.13067313117307, 150.9734433028697}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1464</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{438.38067313117307, 148.2234433028697}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1465</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1461</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1467</integer>
+ <key>Points</key>
+ <array>
+ <string>{526.18734328198354, 242.08376201128482}</string>
+ <string>{524.58556048073308, 240.17656069584442}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{468.44976222297396, 144.11413041602617}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1468</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{440.77214680301904, 150.76115237036112}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1469</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{438.02214680301904, 148.01115237036112}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1470</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1466</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{440.67614795890404, 150.75805299902771}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1471</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.673128</string>
+ <key>g</key>
+ <string>0.534129</string>
+ <key>r</key>
+ <string>0.217952</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>VFlip</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1459</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{324.9947377700048, 478.76189426912856}, {46, 14}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>FitText</key>
+ <string>YES</string>
+ <key>Flow</key>
+ <string>Resize</string>
+ <key>ID</key>
+ <integer>1472</integer>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Pad</key>
+ <integer>0</integer>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\b\fs24 \cf0 OCTO
+\b0 X}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>Wrap</key>
+ <string>NO</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{267.24952227247923, 404.95559172255122}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1474</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 2}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1476</integer>
+ <key>Points</key>
+ <array>
+ <string>{332.396988909622, 413.07125181593568}</string>
+ <string>{329.91576246001244, 413.28721548361688}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{226.49295677281663, 431.38573061302958}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1477</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{228.4458262787623, 366.49842726410935}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1478</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{225.6958262787623, 363.74842726410935}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1479</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1475</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1481</integer>
+ <key>Points</key>
+ <array>
+ <string>{226.20695455654064, 421.06707636999505}</string>
+ <string>{228.68818100615007, 420.8511127023138}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{316.11098669334581, 384.25259757290121}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1482</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{228.65811718740025, 366.13990092182132}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1483</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{225.90811718740025, 363.38990092182132}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1484</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1480</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{228.33784007224426, 366.04390208826476}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1485</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1473</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{181.88306202487647, 189.87345220332375}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1487</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 7}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1489</integer>
+ <key>Points</key>
+ <array>
+ <string>{247.03052866201915, 197.98911229670802}</string>
+ <string>{244.5493022124096, 198.20507596438921}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{141.12649652521372, 216.30359109380208}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1490</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{143.07936603115925, 151.41628774488177}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1491</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{140.32936603115925, 148.66628774488177}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1492</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1488</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1494</integer>
+ <key>Points</key>
+ <array>
+ <string>{140.84049430893771, 205.98493685076758}</string>
+ <string>{143.32172075854714, 205.76897318308625}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{230.74452644574279, 169.17045805367366}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1495</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{143.29165693979732, 151.05776140259383}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1496</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{140.54165693979732, 148.30776140259383}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1497</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1493</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{142.97137982464162, 150.96176256903718}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1498</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1486</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{479.69876456560792, 313.23275528336626}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1500</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 8}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1502</integer>
+ <key>Points</key>
+ <array>
+ <string>{544.84623120275069, 321.34841537675055}</string>
+ <string>{542.36500475314142, 321.56437904443163}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{438.94219906594537, 339.66289417384445}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1503</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{440.89506857189104, 274.77559082492428}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1504</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{438.14506857189104, 272.02559082492428}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1505</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1501</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1507</integer>
+ <key>Points</key>
+ <array>
+ <string>{438.65619684966936, 329.34423993081015}</string>
+ <string>{441.13742329927879, 329.12827626312878}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{528.56022898647439, 292.52976113371614}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1508</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{441.107359480529, 274.41706448263619}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1509</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{438.357359480529, 271.66706448263619}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1510</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1506</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{440.78708236537307, 274.3210656490798}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1511</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1499</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{248.28852278518417, 235.77792148758147}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1512</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{425.23537052568395, 232.45140364934065}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1513</integer>
+ <key>Rotation</key>
+ <real>292.50006103515625</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{374.59821460510483, 282.06732351898592}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1514</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Bounds</key>
+ <string>{{394.33229525377027, 98.150584482954116}, {24, 24}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1516</integer>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>Text</key>
+ <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370
+\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;}
+{\colortbl;\red255\green255\blue255;}
+\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc
+
+\f0\fs24 \cf0 1}</string>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1518</integer>
+ <key>Points</key>
+ <array>
+ <string>{459.47976189091298, 106.26624457633855}</string>
+ <string>{456.99853544130355, 106.48220824401977}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{353.57572975410767, 124.58072337343253}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1519</integer>
+ <key>Rotation</key>
+ <real>67.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{355.52859926005328, 59.693420024512235}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1520</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{352.77859926005328, 56.943420024512235}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1521</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1517</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Class</key>
+ <string>Group</string>
+ <key>Graphics</key>
+ <array>
+ <dict>
+ <key>Class</key>
+ <string>LineGraphic</string>
+ <key>ID</key>
+ <integer>1523</integer>
+ <key>Points</key>
+ <array>
+ <string>{353.28972753783154, 114.26206913039776}</string>
+ <string>{355.77095398744109, 114.04610546271654}</string>
+ </array>
+ <key>Style</key>
+ <dict>
+ <key>stroke</key>
+ <dict>
+ <key>HeadArrow</key>
+ <string>0</string>
+ <key>Legacy</key>
+ <true/>
+ <key>TailArrow</key>
+ <string>0</string>
+ </dict>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{443.19375967463657, 77.447590333303964}, {16, 18.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1524</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>HorizontalTriangle</string>
+ <key>Style</key>
+ <dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{355.74089016869129, 59.33489368222402}, {101.5, 101.5}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1525</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{352.99089016869129, 56.58489368222402}, {107, 107}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>HFlip</key>
+ <string>YES</string>
+ <key>ID</key>
+ <integer>1526</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>AdjustableArc</string>
+ <key>ShapeData</key>
+ <dict>
+ <key>endAngle</key>
+ <real>72</real>
+ <key>startAngle</key>
+ <real>281</real>
+ </dict>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ <key>TextRelativeArea</key>
+ <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1522</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{355.42061305353548, 59.238894848667528}, {101.82337649086284, 101.82337649086284}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1527</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ <key>Shape</key>
+ <string>Circle</string>
+ <key>Style</key>
+ <dict>
+ <key>fill</key>
+ <dict>
+ <key>Color</key>
+ <dict>
+ <key>a</key>
+ <string>0.78</string>
+ <key>b</key>
+ <string>0.234512</string>
+ <key>g</key>
+ <string>0.673128</string>
+ <key>r</key>
+ <string>0.148947</string>
+ </dict>
+ </dict>
+ <key>shadow</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ <key>stroke</key>
+ <dict>
+ <key>Draws</key>
+ <string>NO</string>
+ </dict>
+ </dict>
+ <key>Text</key>
+ <dict>
+ <key>VerticalPad</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1515</integer>
+ <key>Rotation</key>
+ <real>337.5</real>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{252.46989794607327, 160.88960179706885}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1528</integer>
+ <key>Rotation</key>
+ <real>292.50006103515625</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{426.89836479441431, 161.02992820772749}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1529</integer>
+ <key>Rotation</key>
+ <real>247.5</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{302.52032988683095, 284.35823469264687}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1530</integer>
+ <key>Rotation</key>
+ <real>22.500026702880859</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{375.35774119930903, 108.51315184010758}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1531</integer>
+ <key>Rotation</key>
+ <real>22.500026702880859</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ <dict>
+ <key>Bounds</key>
+ <string>{{303.4692327942559, 110.3467678696648}, {8, 134}}</string>
+ <key>Class</key>
+ <string>ShapedGraphic</string>
+ <key>ID</key>
+ <integer>1532</integer>
+ <key>Rotation</key>
+ <real>157.5</real>
+ <key>Shape</key>
+ <string>Rectangle</string>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1419</integer>
+ <key>Rotation</key>
+ <real>45</real>
+ </dict>
+ </array>
+ <key>ID</key>
+ <integer>1415</integer>
+ </dict>
+ </array>
+ <key>GridInfo</key>
+ <dict/>
+ <key>HPages</key>
+ <integer>1</integer>
+ <key>KeepToScale</key>
+ <false/>
+ <key>Layers</key>
+ <array>
+ <dict>
+ <key>Lock</key>
+ <string>NO</string>
+ <key>Name</key>
+ <string>Layer 1</string>
+ <key>Print</key>
+ <string>YES</string>
+ <key>View</key>
+ <string>YES</string>
+ </dict>
+ </array>
+ <key>LayoutInfo</key>
+ <dict>
+ <key>Animate</key>
+ <string>NO</string>
+ <key>circoMinDist</key>
+ <real>18</real>
+ <key>circoSeparation</key>
+ <real>0.0</real>
+ <key>layoutEngine</key>
+ <string>dot</string>
+ <key>neatoSeparation</key>
+ <real>0.0</real>
+ <key>twopiSeparation</key>
+ <real>0.0</real>
+ </dict>
+ <key>Orientation</key>
+ <integer>2</integer>
+ <key>PrintOnePage</key>
+ <false/>
+ <key>RowAlign</key>
+ <integer>1</integer>
+ <key>RowSpacing</key>
+ <real>36</real>
+ <key>SheetTitle</key>
+ <string>Rotor Assignment 3</string>
+ <key>UniqueID</key>
+ <integer>4</integer>
+ <key>VPages</key>
+ <integer>1</integer>
+ </dict>
+ </array>
+ <key>SmartAlignmentGuidesActive</key>
+ <string>YES</string>
+ <key>SmartDistanceGuidesActive</key>
+ <string>YES</string>
+ <key>UseEntirePage</key>
+ <false/>
+ <key>WindowInfo</key>
+ <dict>
+ <key>CurrentSheet</key>
+ <integer>4</integer>
+ <key>ExpandedCanvases</key>
+ <array>
+ <dict>
+ <key>name</key>
+ <string>Canvas 1</string>
+ </dict>
+ </array>
+ <key>Frame</key>
+ <string>{{96, 56}, {1043, 822}}</string>
+ <key>ListView</key>
+ <true/>
+ <key>OutlineWidth</key>
+ <integer>142</integer>
+ <key>RightSidebar</key>
+ <false/>
+ <key>ShowRuler</key>
+ <true/>
+ <key>Sidebar</key>
+ <true/>
+ <key>SidebarWidth</key>
+ <integer>120</integer>
+ <key>VisibleRegion</key>
+ <string>{{-51, -33}, {908, 683}}</string>
+ <key>Zoom</key>
+ <real>1</real>
+ <key>ZoomValues</key>
+ <array>
+ <array>
+ <string>Canvas 1</string>
+ <real>1</real>
+ <real>0.5</real>
+ </array>
+ <array>
+ <string>Rotor Assignments</string>
+ <real>1</real>
+ <real>4</real>
+ </array>
+ <array>
+ <string>Rotor Assignment 2</string>
+ <real>1</real>
+ <real>4</real>
+ </array>
+ <array>
+ <string>Rotor Assignment 3</string>
+ <real>1</real>
+ <real>1</real>
+ </array>
+ <array>
+ <string>Remote Control</string>
+ <real>1</real>
+ <real>1</real>
+ </array>
+ </array>
+ </dict>
+</dict>
+</plist>
diff --git a/Makefile b/Makefile
index e9442afd2..1d1287b7f 100644
--- a/Makefile
+++ b/Makefile
@@ -1,43 +1,53 @@
#
-# Top-level Makefile for building PX4 firmware images.
-#
-#
-# Note that this is a transitional process; the eventual goal is for this
-# project to slim down and simply generate PX4 link kits via the NuttX
-# 'make export' mechanism.
-#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
#
#
-# Some useful paths.
+# Top-level Makefile for building PX4 firmware images.
#
-export PX4BASE = $(realpath $(dir $(lastword $(MAKEFILE_LIST))))
-export NUTTX_SRC = $(PX4BASE)/nuttx
-export NUTTX_APPS = $(PX4BASE)/apps
-export MAVLINK_SRC = $(PX4BASE)/mavlink
-export ROMFS_SRC = $(PX4BASE)/ROMFS
-export IMAGE_DIR = $(PX4BASE)/Images
#
-# Tools
+# Get path and tool configuration
#
-MKFW = $(PX4BASE)/Tools/px_mkfw.py
-UPLOADER = $(PX4BASE)/Tools/px_uploader.py
+export PX4_BASE := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/
+include $(PX4_BASE)makefiles/setup.mk
#
-# What are we currently configured for?
+# Canned firmware configurations that we build.
#
-CONFIGURED = $(PX4BASE)/.configured
-ifneq ($(wildcard $(CONFIGURED)),)
-export TARGET := $(shell cat $(CONFIGURED))
-endif
+CONFIGS ?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk))))
#
-# What we will build
+# Boards that we build NuttX export kits for.
#
-FIRMWARE_BUNDLE = $(IMAGE_DIR)/$(TARGET).px4
-FIRMWARE_BINARY = $(IMAGE_DIR)/$(TARGET).bin
-FIRMWARE_PROTOTYPE = $(IMAGE_DIR)/$(TARGET).prototype
+BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk))))
#
# Debugging
@@ -45,120 +55,156 @@ FIRMWARE_PROTOTYPE = $(IMAGE_DIR)/$(TARGET).prototype
MQUIET = --no-print-directory
#MQUIET = --print-directory
-all: $(FIRMWARE_BUNDLE)
+################################################################################
+# No user-serviceable parts below
+################################################################################
#
-# Generate a wrapped .px4 file from the built binary
+# If the user has listed a config as a target, strip it out and override CONFIGS.
#
-$(FIRMWARE_BUNDLE): $(FIRMWARE_BINARY) $(MKFW) $(FIRMWARE_PROTOTYPE)
- @echo Generating $@
- @$(MKFW) --prototype $(FIRMWARE_PROTOTYPE) \
- --git_identity $(PX4BASE) \
- --image $(FIRMWARE_BINARY) > $@
+FIRMWARE_GOAL = firmware
+EXPLICIT_CONFIGS := $(filter $(CONFIGS),$(MAKECMDGOALS))
+ifneq ($(EXPLICIT_CONFIGS),)
+CONFIGS := $(EXPLICIT_CONFIGS)
+.PHONY: $(EXPLICIT_CONFIGS)
+$(EXPLICIT_CONFIGS): all
#
-# Build the firmware binary.
+# If the user has asked to upload, they must have also specified exactly one
+# config.
#
-.PHONY: $(FIRMWARE_BINARY)
-$(FIRMWARE_BINARY): setup_$(TARGET) configure-check
- @echo Building $@ for $(TARGET)
- @make -C $(NUTTX_SRC) -r $(MQUIET) all
- @cp $(NUTTX_SRC)/nuttx.bin $@
+ifneq ($(filter upload,$(MAKECMDGOALS)),)
+ifneq ($(words $(EXPLICIT_CONFIGS)),1)
+$(error In order to upload, exactly one board config must be specified)
+endif
+FIRMWARE_GOAL = upload
+.PHONY: upload
+upload:
+ @:
+endif
+endif
#
-# The 'configure' targets select one particular firmware configuration
-# and makes it current.
+# Built products
#
-configure_px4fmu:
- @echo Configuring for px4fmu
- @make -C $(PX4BASE) distclean
- @cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4fmu/nsh
- @echo px4fmu > $(CONFIGURED)
-
-configure_px4io:
- @echo Configuring for px4io
- @make -C $(PX4BASE) distclean
- @cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4io/io
- @echo px4io > $(CONFIGURED)
-
-configure-check:
-ifeq ($(wildcard $(CONFIGURED)),)
- @echo
- @echo "Not configured - use 'make configure_px4fmu' or 'make configure_px4io' first"
- @echo
- @exit 1
-endif
+STAGED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
+FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
+all: $(STAGED_FIRMWARES)
#
-# Per-configuration additional targets
+# Copy FIRMWARES into the image directory.
#
-.PHONY: px4fmu_setup
-setup_px4fmu:
- @echo Generating ROMFS
- @make -C $(ROMFS_SRC) all
-
-setup_px4io:
-
-# fake target to make configure-check happy if TARGET is not set
-setup_:
+$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
+ @echo %% Copying $@
+ $(Q) $(COPY) $< $@
#
-# Firmware uploading.
+# Generate FIRMWARES.
#
-
-# serial port defaults by operating system.
-SYSTYPE = $(shell uname)
-ifeq ($(SYSTYPE),Darwin)
-SERIAL_PORTS ?= "/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4,/dev/tty.usbmodem1,/dev/tty.usbmodem2,/dev/tty.usbmodem3,/dev/tty.usbmodem4"
-endif
-ifeq ($(SYSTYPE),Linux)
-SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0"
-endif
-ifeq ($(SERIAL_PORTS),)
-SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0"
-endif
-
-upload: $(FIRMWARE_BUNDLE) $(UPLOADER)
- $(UPLOADER) --port $(SERIAL_PORTS) $(FIRMWARE_BUNDLE)
+.PHONY: $(FIRMWARES)
+$(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@)
+$(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/
+$(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:
+ @echo %%%%
+ @echo %%%% Building $(config) in $(work_dir)
+ @echo %%%%
+ $(Q) mkdir -p $(work_dir)
+ $(Q) make -r -C $(work_dir) \
+ -f $(PX4_MK_DIR)firmware.mk \
+ CONFIG=$(config) \
+ WORK_DIR=$(work_dir) \
+ $(FIRMWARE_GOAL)
#
-# JTAG firmware uploading with OpenOCD
+# Build the NuttX export archives.
#
-ifeq ($(JTAGCONFIG),)
-JTAGCONFIG=interface/olimex-jtag-tiny.cfg
-endif
-
-upload-jtag-px4fmu:
- @echo Attempting to flash PX4FMU board via JTAG
- @openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
-
-upload-jtag-px4io: all
- @echo Attempting to flash PX4IO board via JTAG
- @openocd -f $(JTAGCONFIG) -f ../Bootloader/stm32f1x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4io_bl.elf" -c "reset run" -c shutdown
-
+# Note that there are no explicit dependencies extended from these
+# archives. If NuttX is updated, the user is expected to rebuild the
+# archives/build area manually. Likewise, when the 'archives' target is
+# invoked, all archives are always rebuilt.
#
-# Hacks and fixups
+# XXX Should support fetching/unpacking from a separate directory to permit
+# downloads of the prebuilt archives as well...
#
+# XXX PX4IO configuration name is bad - NuttX configs should probably all be "px4"
+#
+NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export)
+.PHONY: archives
+archives: $(NUTTX_ARCHIVES)
-ifeq ($(SYSTYPE),Darwin)
-# PATH inherited by Eclipse may not include toolchain install location
-export PATH := $(PATH):/usr/local/bin
+# We cannot build these parallel; note that we also force -j1 for the
+# sub-make invocations.
+ifneq ($(filter archives,$(MAKECMDGOALS)),)
+.NOTPARALLEL:
endif
+$(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@))
+$(ARCHIVE_DIR)%.export: configuration = $(if $(filter $(board),px4io),io,nsh)
+$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS)
+ @echo %% Configuring NuttX for $(board)
+ $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
+ $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean
+ $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration))
+ @echo %% Exporting NuttX for $(board)
+ $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) export
+ $(Q) mkdir -p $(dir $@)
+ $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@
+
#
# Cleanup targets. 'clean' should remove all built products and force
# a complete re-compilation, 'distclean' should remove everything
# that's generated leaving only files that are in source control.
#
-.PHONY: clean upload-jtag-px4fmu
+.PHONY: clean
clean:
- @make -C $(NUTTX_SRC) -r $(MQUIET) distclean
- @make -C $(ROMFS_SRC) -r $(MQUIET) clean
+ $(Q) $(RMDIR) $(BUILD_DIR)*.build
+ $(Q) $(REMOVE) $(IMAGE_DIR)*.px4
.PHONY: distclean
-distclean:
- @rm -f $(CONFIGURED)
- @make -C $(NUTTX_SRC) -r $(MQUIET) distclean
- @make -C $(ROMFS_SRC) -r $(MQUIET) distclean
-
+distclean: clean
+ $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
+ $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
+
+#
+# Print some help text
+#
+.PHONY: help
+help:
+ @echo ""
+ @echo " PX4 firmware builder"
+ @echo " ===================="
+ @echo ""
+ @echo " Available targets:"
+ @echo " ------------------"
+ @echo ""
+ @echo " archives"
+ @echo " Build the NuttX RTOS archives that are used by the firmware build."
+ @echo ""
+ @echo " all"
+ @echo " Build all firmware configs: $(CONFIGS)"
+ @echo " A limited set of configs can be built with CONFIGS=<list-of-configs>"
+ @echo ""
+ @for config in $(CONFIGS); do \
+ echo " $$config"; \
+ echo " Build just the $$config firmware configuration."; \
+ echo ""; \
+ done
+ @echo " clean"
+ @echo " Remove all firmware build pieces."
+ @echo ""
+ @echo " distclean"
+ @echo " Remove all compilation products, including NuttX RTOS archives."
+ @echo ""
+ @echo " upload"
+ @echo " When exactly one config is being built, add this target to upload the"
+ @echo " firmware to the board when the build is complete. Not supported for"
+ @echo " all configurations."
+ @echo ""
+ @echo " Common options:"
+ @echo " ---------------"
+ @echo ""
+ @echo " V=1"
+ @echo " If V is set, more verbose output is printed during the build. This can"
+ @echo " help when diagnosing issues with the build or toolchain."
+ @echo ""
diff --git a/ROMFS/.gitignore b/ROMFS/.gitignore
deleted file mode 100644
index 30d3d7fe5..000000000
--- a/ROMFS/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-/img
diff --git a/ROMFS/Makefile b/ROMFS/Makefile
deleted file mode 100644
index ed39ab825..000000000
--- a/ROMFS/Makefile
+++ /dev/null
@@ -1,122 +0,0 @@
-#
-# Makefile to generate a PX4FMU ROMFS image.
-#
-# In normal use, 'make install' will generate a new ROMFS header and place it
-# into the px4fmu configuration in the appropriate location.
-#
-
-#
-# Directories of interest
-#
-SRCROOT ?= $(dir $(lastword $(MAKEFILE_LIST)))
-BUILDROOT ?= $(SRCROOT)/img
-ROMFS_HEADER ?= $(SRCROOT)/../nuttx/configs/px4fmu/include/nsh_romfsimg.h
-
-#
-# List of files to install in the ROMFS, specified as <source>~<destination>
-#
-ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \
- $(SRCROOT)/scripts/rc.sensors~init.d/rc.sensors \
- $(SRCROOT)/scripts/rc.logging~init.d/rc.logging \
- $(SRCROOT)/scripts/rc.standalone~init.d/rc.standalone \
- $(SRCROOT)/scripts/rc.PX4IO~init.d/rc.PX4IO \
- $(SRCROOT)/scripts/rc.PX4IOAR~init.d/rc.PX4IOAR \
- $(SRCROOT)/scripts/rc.FMU_quad_x~init.d/rc.FMU_quad_x \
- $(SRCROOT)/scripts/rc.usb~init.d/rc.usb \
- $(SRCROOT)/scripts/rc.hil~init.d/rc.hil \
- $(SRCROOT)/mixers/FMU_pass.mix~mixers/FMU_pass.mix \
- $(SRCROOT)/mixers/FMU_Q.mix~mixers/FMU_Q.mix \
- $(SRCROOT)/mixers/FMU_X5.mix~mixers/FMU_X5.mix \
- $(SRCROOT)/mixers/FMU_AERT.mix~mixers/FMU_AERT.mix \
- $(SRCROOT)/mixers/FMU_AET.mix~mixers/FMU_AET.mix \
- $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \
- $(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \
- $(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix \
- $(SRCROOT)/mixers/FMU_hex_x.mix~mixers/FMU_hex_x.mix \
- $(SRCROOT)/mixers/FMU_hex_+.mix~mixers/FMU_hex_+.mix \
- $(SRCROOT)/mixers/FMU_octo_x.mix~mixers/FMU_octo_x.mix \
- $(SRCROOT)/mixers/FMU_octo_+.mix~mixers/FMU_octo_+.mix \
- $(SRCROOT)/logging/logconv.m~logging/logconv.m
-
-# the EXTERNAL_SCRIPTS variable is used to add out of tree scripts
-# to ROMFS.
-ROMFS_FSSPEC += $(EXTERNAL_SCRIPTS)
-
-#
-# Add the PX4IO firmware to the spec if someone has dropped it into the
-# source directory, or otherwise specified its location.
-#
-# Normally this is only something you'd do when working on PX4IO; most
-# users will upgrade with firmware off the microSD card.
-#
-PX4IO_FIRMWARE ?= $(SRCROOT)/px4io.bin
-ifneq ($(wildcard $(PX4IO_FIRMWARE)),)
-ROMFS_FSSPEC += $(PX4IO_FIRMWARE)~px4io.bin
-endif
-
-################################################################################
-# No user-serviceable parts below
-################################################################################
-
-#
-# Just the source files from the ROMFS spec, so that we can fail cleanly if they don't
-# exist
-#
-ROMFS_SRCFILES = $(foreach spec,$(ROMFS_FSSPEC),$(firstword $(subst ~, ,$(spec))))
-
-#
-# Just the destination directories from the ROMFS spec
-#
-ROMFS_DIRS = $(sort $(dir $(foreach spec,$(ROMFS_FSSPEC),$(lastword $(subst ~, ,$(spec))))))
-
-
-#
-# Intermediate products
-#
-ROMFS_IMG = $(BUILDROOT)/romfs.img
-ROMFS_WORKDIR = $(BUILDROOT)/romfs
-
-#
-# Convenience target for rebuilding the ROMFS header
-#
-all: $(ROMFS_HEADER)
-
-$(ROMFS_HEADER): $(ROMFS_IMG) $(dir $(ROMFS_HEADER))
- @echo Generating the ROMFS header...
- @(cd $(dir $(ROMFS_IMG)) && xxd -i $(notdir $(ROMFS_IMG))) | sed -e 's/char/const char/' > $@
-
-$(ROMFS_IMG): $(ROMFS_WORKDIR)
- @echo Generating the ROMFS image...
- @genromfs -f $@ -d $(ROMFS_WORKDIR) -V "NSHInitVol"
-
-$(ROMFS_WORKDIR): $(ROMFS_SRCFILES)
- @echo Rebuilding the ROMFS work area...
- @rm -rf $(ROMFS_WORKDIR)
- @mkdir -p $(ROMFS_WORKDIR)
- @for dir in $(ROMFS_DIRS) ; do mkdir -p $(ROMFS_WORKDIR)/$$dir; done
- @for spec in $(ROMFS_FSSPEC) ; do \
- echo $$spec | sed -e 's%^.*~% %' ;\
- `echo "cp $$spec" | sed -e 's%~% $(ROMFS_WORKDIR)/%'` ;\
- done
-
-$(BUILDROOT):
- @mkdir -p $(BUILDROOT)
-
-clean:
- @rm -rf $(BUILDROOT)
-
-distclean: clean
- @rm -f $(PX4IO_FIRMWARE) $(ROMFS_HEADER)
-
-.PHONY: all install clean distclean
-
-#
-# Hacks and fixups
-#
-SYSTYPE = $(shell uname)
-
-ifeq ($(SYSTYPE),Darwin)
-# PATH inherited by Eclipse may not include toolchain install location
-export PATH := $(PATH):/usr/local/bin
-endif
-
diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m
deleted file mode 100755
index 92ee01413..000000000
--- a/ROMFS/logging/logconv.m
+++ /dev/null
@@ -1,224 +0,0 @@
-% This Matlab Script can be used to import the binary logged values of the
-% PX4FMU into data that can be plotted and analyzed.
-
-% Clear everything
-clc
-clear all
-close all
-
-% Set the path to your sysvector.bin file here
-filePath = 'sysvector.bin';
-
-% Work around a Matlab bug (not related to PX4)
-% where timestamps from 1.1.1970 do not allow to
-% read the file's size
-if ismac
- system('touch -t 201212121212.12 sysvector.bin');
-end
-
-%%%%%%%%%%%%%%%%%%%%%%%
-% SYSTEM VECTOR
-%
-% //All measurements in NED frame
-%
-% uint64_t timestamp; //[us]
-% float gyro[3]; //[rad/s]
-% float accel[3]; //[m/s^2]
-% float mag[3]; //[gauss]
-% float baro; //pressure [millibar]
-% float baro_alt; //altitude above MSL [meter]
-% float baro_temp; //[degree celcius]
-% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
-% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
-% float vbat; //battery voltage in [volt]
-% float bat_current - current drawn from battery at this time instant
-% float bat_discharged - discharged energy in mAh
-% float adc[4]; //ADC ports [volt]
-% float local_position[3]; //tangent plane mapping into x,y,z [m]
-% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
-% float attitude[3]; //pitch, roll, yaw [rad]
-% float rotMatrix[9]; //unitvectors
-% float actuator_control[4]; //unitvector
-% float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
-% float diff_pressure; - pressure difference in millibar
-% float ind_airspeed;
-% float true_airspeed;
-
-% Definition of the logged values
-logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64');
-logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
-logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-
-% First get length of one line
-columns = length(logFormat);
-lineLength = 0;
-
-for i=1:columns
- lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array;
-end
-
-
-if exist(filePath, 'file')
-
- fileInfo = dir(filePath);
- fileSize = fileInfo.bytes;
-
- elements = int64(fileSize./(lineLength));
-
- fid = fopen(filePath, 'r');
- offset = 0;
- for i=1:columns
- % using fread with a skip speeds up the import drastically, do not
- % import the values one after the other
- sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(...
- fid, ...
- [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ...
- lineLength - logFormat{i}.bytes*logFormat{i}.array, ...
- logFormat{i}.machineformat) ...
- );
- offset = offset + logFormat{i}.bytes*logFormat{i}.array;
- fseek(fid, offset,'bof');
- end
-
- % shot the flight time
- time_us = sysvector.timestamp(end) - sysvector.timestamp(1);
- time_s = time_us*1e-6;
- time_m = time_s/60;
-
- % close the logfile
- fclose(fid);
-
- disp(['end log2matlab conversion' char(10)]);
-else
- disp(['file: ' filePath ' does not exist' char(10)]);
-end
-
-%% Plot GPS RAW measurements
-
-% Only plot GPS data if available
-if cumsum(double(sysvector.gps_raw_position(200:end,1))) > 0
- figure('units','normalized','outerposition',[0 0 1 1])
- plot3(sysvector.gps_raw_position(200:end,1), sysvector.gps_raw_position(200:end,2), sysvector.gps_raw_position(200:end,3));
-end
-
-
-%% Plot optical flow trajectory
-
-flow_sz = size(sysvector.timestamp);
-flow_elements = flow_sz(1);
-
-xt(1:flow_elements,1) = sysvector.timestamp(:,1); % time column [ms]
-
-
-%calc dt
-dt = zeros(flow_elements,1);
-for i = 1:flow_elements-1
- dt(i+1,1) = double(xt(i+1,1)-xt(i,1)) * 10^(-6); % timestep [s]
-end
-dt(1,1) = mean(dt);
-
-
-global_speed = zeros(flow_elements,3);
-
-%calc global speed (with rot matrix)
-for i = 1:flow_elements
- rotM = [sysvector.rot_matrix(i,1:3);sysvector.rot_matrix(i,4:6);sysvector.rot_matrix(i,7:9)]';
- speedX = sysvector.optical_flow(i,3);
- speedY = sysvector.optical_flow(i,4);
-
- relSpeed = [-speedY,speedX,0];
- global_speed(i,:) = relSpeed * rotM;
-end
-
-
-
-px = zeros(flow_elements,1);
-py = zeros(flow_elements,1);
-distance = 0;
-
-last_vx = 0;
-last_vy = 0;
-elem_cnt = 0;
-
-% Very basic accumulation, stops on bad flow quality
-for i = 1:flow_elements
- if sysvector.optical_flow(i,6) > 5
- px(i,1) = global_speed(i,1)*dt(i,1);
- py(i,1) = global_speed(i,2)*dt(i,1);
- distance = distance + norm([px(i,1) py(i,1)]);
- last_vx = px(i,1);
- last_vy = py(i,1);
- else
- px(i,1) = last_vx;
- py(i,1) = last_vy;
- last_vx = last_vx*0.95;
- last_vy = last_vy*0.95;
- end
-end
-
-px_sum = cumsum(px);
-py_sum = cumsum(py);
-time = cumsum(dt);
-
-figure()
-set(gca, 'Units','normal');
-
-plot(py_sum, px_sum, '-blue', 'LineWidth',2);
-axis equal;
-% set title and axis captions
-xlabel('X position (meters)','fontsize',14)
-ylabel('Y position (meters)','fontsize',14)
-% mark begin and end
-hold on
-plot(py_sum(1,1),px_sum(1,1),'ks','LineWidth',2,...
-'MarkerEdgeColor','k',...
-'MarkerFaceColor','g',...
-'MarkerSize',10)
-hold on
-plot(py_sum(end,1),px_sum(end,1),'kv','LineWidth',2,...
-'MarkerEdgeColor','k',...
-'MarkerFaceColor','b',...
-'MarkerSize',10)
-% add total length as annotation
-set(gca,'fontsize',13);
-legend('Trajectory', 'START', sprintf('END\n(%.2f m, %.0f:%.0f s)', distance, time_m, time_s - time_m*60));
-title('Optical Flow Position Integration', 'fontsize', 15);
-
-figure()
-plot(time, sysvector.optical_flow(:,5), 'blue');
-axis([time(1,1) time(end,1) 0 (max(sysvector.optical_flow(i,5))+0.2)]);
-xlabel('seconds','fontsize',14);
-ylabel('m','fontsize',14);
-set(gca,'fontsize',13);
-title('Ultrasound Altitude', 'fontsize', 15);
-
-
-figure()
-plot(time, global_speed(:,2), 'red');
-hold on;
-plot(time, global_speed(:,1), 'blue');
-legend('y velocity (m/s)', 'x velocity (m/s)');
-xlabel('seconds','fontsize',14);
-ylabel('m/s','fontsize',14);
-set(gca,'fontsize',13);
-title('Optical Flow Velocity', 'fontsize', 15);
diff --git a/ROMFS/scripts/rc.FMU_quad_x b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x
index 8787443ea..980197d68 100644
--- a/ROMFS/scripts/rc.FMU_quad_x
+++ b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x
@@ -20,10 +20,10 @@ uorb start
# Load microSD params
#
echo "[init] loading microSD params"
-param select /fs/microsd/parameters
-if [ -f /fs/microsd/parameters ]
+param select /fs/microsd/params
+if [ -f /fs/microsd/params ]
then
- param load /fs/microsd/parameters
+ param load /fs/microsd/params
fi
#
diff --git a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD b/ROMFS/px4fmu_common/init.d/rc.IO_QUAD
new file mode 100644
index 000000000..5f2de0d7e
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.IO_QUAD
@@ -0,0 +1,107 @@
+#!nsh
+
+# Disable USB and autostart
+set USB no
+set MODE quad
+
+#
+# Start the ORB (first app to start)
+#
+uorb start
+
+#
+# Load microSD params
+#
+echo "[init] loading microSD params"
+param select /fs/microsd/params
+if [ -f /fs/microsd/params ]
+then
+ param load /fs/microsd/params
+fi
+
+#
+# Force some key parameters to sane values
+# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
+# see https://pixhawk.ethz.ch/mavlink/
+#
+param set MAV_TYPE 2
+
+#
+# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
+#
+if [ -f /fs/microsd/px4io.bin ]
+then
+ echo "PX4IO Firmware found. Checking Upgrade.."
+ if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+ then
+ echo "No newer version, skipping upgrade."
+ else
+ echo "Loading /fs/microsd/px4io.bin"
+ if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
+ then
+ cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+ echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
+ else
+ echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
+ echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
+ fi
+ fi
+fi
+
+#
+# Start MAVLink (depends on orb)
+#
+mavlink start -d /dev/ttyS1 -b 57600
+usleep 5000
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# Start PX4IO interface (depends on orb, commander)
+#
+px4io start
+
+#
+# Allow PX4IO to recover from midair restarts.
+# this is very unlikely, but quite safe and robust.
+px4io recovery
+
+#
+# Start the sensors (depends on orb, px4io)
+#
+sh /etc/init.d/rc.sensors
+
+#
+# Start GPS interface (depends on orb)
+#
+gps start
+
+#
+# Start the attitude estimator (depends on orb)
+#
+attitude_estimator_ekf start
+
+#
+# Load mixer and start controllers (depends on px4io)
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
+multirotor_att_control start
+
+#
+# Start logging
+#
+#sdlog start -s 4
+
+#
+# Start system state
+#
+if blinkm start
+then
+ echo "using BlinkM for state indication"
+ blinkm systemstate
+else
+ echo "no BlinkM found, OK."
+fi \ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/rc.PX4IO
new file mode 100644
index 000000000..925a5703e
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.PX4IO
@@ -0,0 +1,107 @@
+#!nsh
+
+# Disable USB and autostart
+set USB no
+set MODE camflyer
+
+#
+# Start the ORB (first app to start)
+#
+uorb start
+
+#
+# Load microSD params
+#
+echo "[init] loading microSD params"
+param select /fs/microsd/params
+if [ -f /fs/microsd/params ]
+then
+ param load /fs/microsd/params
+fi
+
+#
+# Force some key parameters to sane values
+# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
+# see https://pixhawk.ethz.ch/mavlink/
+#
+param set MAV_TYPE 1
+
+#
+# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
+#
+if [ -f /fs/microsd/px4io.bin ]
+then
+ echo "PX4IO Firmware found. Checking Upgrade.."
+ if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+ then
+ echo "No newer version, skipping upgrade."
+ else
+ echo "Loading /fs/microsd/px4io.bin"
+ if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
+ then
+ cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+ echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
+ else
+ echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
+ echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
+ fi
+ fi
+fi
+
+#
+# Start MAVLink (depends on orb)
+#
+mavlink start -d /dev/ttyS1 -b 57600
+usleep 5000
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# Start PX4IO interface (depends on orb, commander)
+#
+px4io start
+
+#
+# Allow PX4IO to recover from midair restarts.
+# this is very unlikely, but quite safe and robust.
+px4io recovery
+
+#
+# Start the sensors (depends on orb, px4io)
+#
+sh /etc/init.d/rc.sensors
+
+#
+# Start GPS interface (depends on orb)
+#
+gps start
+
+#
+# Start the attitude estimator (depends on orb)
+#
+kalman_demo start
+
+#
+# Load mixer and start controllers (depends on px4io)
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
+control_demo start
+
+#
+# Start logging
+#
+#sdlog start -s 4
+
+#
+# Start system state
+#
+if blinkm start
+then
+ echo "using BlinkM for state indication"
+ blinkm systemstate
+else
+ echo "no BlinkM found, OK."
+fi
diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR
index 72df68e35..f55ac2ae3 100644
--- a/ROMFS/scripts/rc.PX4IOAR
+++ b/ROMFS/px4fmu_common/init.d/rc.PX4IOAR
@@ -2,10 +2,10 @@
#
# Flight startup script for PX4FMU on PX4IOAR carrier board.
#
-
+
# Disable the USB interface
set USB no
-
+
# Disable autostarting other apps
set MODE ardrone
@@ -17,21 +17,26 @@ echo "[init] doing PX4IOAR startup..."
uorb start
#
-# Init the parameter storage
+# Load microSD params
#
echo "[init] loading microSD params"
-param select /fs/microsd/parameters
-if [ -f /fs/microsd/parameters ]
+param select /fs/microsd/params
+if [ -f /fs/microsd/params ]
then
- param load /fs/microsd/parameters
+ param load /fs/microsd/params
fi
-
+
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 2
+
+#
+# Configure PX4FMU for operation with PX4IOAR
+#
+fmu mode_gpio_serial
#
# Start the sensors.
@@ -55,11 +60,6 @@ commander start
attitude_estimator_ekf start
#
-# Configure PX4FMU for operation with PX4IOAR
-#
-fmu mode_gpio_serial
-
-#
# Fire up the multi rotor attitude controller
#
multirotor_att_control start
@@ -68,11 +68,6 @@ multirotor_att_control start
# Fire up the AR.Drone interface.
#
ardrone_interface start -d /dev/ttyS1
-
-#
-# Start GPS capture
-#
-gps start
#
# Start logging
@@ -80,6 +75,11 @@ gps start
sdlog start -s 10
#
+# Start GPS capture
+#
+gps start
+
+#
# Start system state
#
if blinkm start
@@ -95,4 +95,5 @@ fi
# use the same UART for telemetry
#
echo "[init] startup done"
-exit \ No newline at end of file
+
+exit
diff --git a/ROMFS/scripts/rc.boarddetect b/ROMFS/px4fmu_common/init.d/rc.boarddetect
index f233e51df..f233e51df 100644
--- a/ROMFS/scripts/rc.boarddetect
+++ b/ROMFS/px4fmu_common/init.d/rc.boarddetect
diff --git a/ROMFS/scripts/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil
index 3b37ac26b..7614ac0fe 100644
--- a/ROMFS/scripts/rc.hil
+++ b/ROMFS/px4fmu_common/init.d/rc.hil
@@ -17,10 +17,10 @@ hil mode_pwm
# Load microSD params
#
echo "[init] loading microSD params"
-param select /fs/microsd/parameters
-if [ -f /fs/microsd/parameters ]
+param select /fs/microsd/params
+if [ -f /fs/microsd/params ]
then
- param load /fs/microsd/parameters
+ param load /fs/microsd/params
fi
#
@@ -36,6 +36,17 @@ param set MAV_TYPE 1
commander start
#
+# Check if we got an IO
+#
+if [ px4io start ]
+then
+ echo "IO started"
+else
+ fmu mode_serial
+ echo "FMU started"
+end
+
+#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
diff --git a/ROMFS/scripts/rc.jig b/ROMFS/px4fmu_common/init.d/rc.jig
index e2b5d8f30..e2b5d8f30 100644
--- a/ROMFS/scripts/rc.jig
+++ b/ROMFS/px4fmu_common/init.d/rc.jig
diff --git a/ROMFS/scripts/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index 09c2d00d1..09c2d00d1 100644
--- a/ROMFS/scripts/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
diff --git a/ROMFS/scripts/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 42c2f52e9..62c7184b8 100644
--- a/ROMFS/scripts/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -7,6 +7,14 @@
# Start sensor drivers here.
#
+#
+# Check for UORB
+#
+if uorb start
+then
+ echo "uORB started"
+fi
+
ms5611 start
adc start
diff --git a/ROMFS/scripts/rc.standalone b/ROMFS/px4fmu_common/init.d/rc.standalone
index 67e95215b..67e95215b 100644
--- a/ROMFS/scripts/rc.standalone
+++ b/ROMFS/px4fmu_common/init.d/rc.standalone
diff --git a/ROMFS/scripts/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 31af3991a..31af3991a 100644
--- a/ROMFS/scripts/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
diff --git a/ROMFS/scripts/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 89a767879..c0a70f7dd 100755
--- a/ROMFS/scripts/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -21,9 +21,9 @@ set MODE autostart
set USB autoconnect
#
-# Start playing the startup tune
+
#
-tone_alarm start
+
#
# Try to mount the microSD card.
@@ -32,8 +32,12 @@ echo "[init] looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
echo "[init] card mounted at /fs/microsd"
+ # Start playing the startup tune
+ tone_alarm start
else
echo "[init] no microSD card found"
+ # Play SOS
+ tone_alarm 2
fi
#
diff --git a/ROMFS/px4fmu_common/logging/logconv.m b/ROMFS/px4fmu_common/logging/logconv.m
new file mode 100644
index 000000000..3750ddae2
--- /dev/null
+++ b/ROMFS/px4fmu_common/logging/logconv.m
@@ -0,0 +1,586 @@
+% This Matlab Script can be used to import the binary logged values of the
+% PX4FMU into data that can be plotted and analyzed.
+
+%% ************************************************************************
+% PX4LOG_PLOTSCRIPT: Main function
+% ************************************************************************
+function PX4Log_Plotscript
+
+% Clear everything
+clc
+clear all
+close all
+
+% ************************************************************************
+% SETTINGS
+% ************************************************************************
+
+% Set the path to your sysvector.bin file here
+filePath = 'sysvector.bin';
+
+% Set the minimum and maximum times to plot here [in seconds]
+mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start]
+maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end]
+
+%Determine which data to plot. Not completely implemented yet.
+bDisplayGPS=true;
+
+%conversion factors
+fconv_gpsalt=1E-3; %[mm] to [m]
+fconv_gpslatlong=1E-7; %[gps_raw_position_unit] to [deg]
+fconv_timestamp=1E-6; % [microseconds] to [seconds]
+
+% ************************************************************************
+% Import the PX4 logs
+% ************************************************************************
+ImportPX4LogData();
+
+%Translate min and max plot times to indices
+time=double(sysvector.timestamp) .*fconv_timestamp;
+mintime_log=time(1); %The minimum time/timestamp found in the log
+maxtime_log=time(end); %The maximum time/timestamp found in the log
+CurTime=mintime_log; %The current time at which to draw the aircraft position
+
+[imintime,imaxtime]=FindMinMaxTimeIndices();
+
+% ************************************************************************
+% PLOT & GUI SETUP
+% ************************************************************************
+NrFigures=5;
+NrAxes=10;
+h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively
+h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively
+h.pathpoints=[]; % Temporary initiliazation of path points
+
+% Setup the GUI to control the plots
+InitControlGUI();
+% Setup the plotting-GUI (figures, axes) itself.
+InitPlotGUI();
+
+% ************************************************************************
+% DRAW EVERYTHING
+% ************************************************************************
+DrawRawData();
+DrawCurrentAircraftState();
+
+%% ************************************************************************
+% *** END OF MAIN SCRIPT ***
+% NESTED FUNCTION DEFINTIONS FROM HERE ON
+% ************************************************************************
+
+
+%% ************************************************************************
+% IMPORTPX4LOGDATA (nested function)
+% ************************************************************************
+% Attention: This is the import routine for firmware from ca. 03/2013.
+% Other firmware versions might require different import
+% routines.
+
+function ImportPX4LogData()
+ % Work around a Matlab bug (not related to PX4)
+ % where timestamps from 1.1.1970 do not allow to
+ % read the file's size
+ if ismac
+ system('touch -t 201212121212.12 sysvector.bin');
+ end
+
+ % ************************************************************************
+ % RETRIEVE SYSTEM VECTOR
+ % *************************************************************************
+ % //All measurements in NED frame
+ %
+ % uint64_t timestamp; //[us]
+ % float gyro[3]; //[rad/s]
+ % float accel[3]; //[m/s^2]
+ % float mag[3]; //[gauss]
+ % float baro; //pressure [millibar]
+ % float baro_alt; //altitude above MSL [meter]
+ % float baro_temp; //[degree celcius]
+ % float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
+ % float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
+ % float vbat; //battery voltage in [volt]
+ % float bat_current - current drawn from battery at this time instant
+ % float bat_discharged - discharged energy in mAh
+ % float adc[4]; //remaining auxiliary ADC ports [volt]
+ % float local_position[3]; //tangent plane mapping into x,y,z [m]
+ % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
+ % float attitude[3]; //pitch, roll, yaw [rad]
+ % float rotMatrix[9]; //unitvectors
+ % float actuator_control[4]; //unitvector
+ % float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
+ % float diff_pressure; - pressure difference in millibar
+ % float ind_airspeed;
+ % float true_airspeed;
+
+ % Definition of the logged values
+ logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64');
+ logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
+ logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+ logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+
+ % First get length of one line
+ columns = length(logFormat);
+ lineLength = 0;
+
+ for i=1:columns
+ lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array;
+ end
+
+
+ if exist(filePath, 'file')
+
+ fileInfo = dir(filePath);
+ fileSize = fileInfo.bytes;
+
+ elements = int64(fileSize./(lineLength));
+
+ fid = fopen(filePath, 'r');
+ offset = 0;
+ for i=1:columns
+ % using fread with a skip speeds up the import drastically, do not
+ % import the values one after the other
+ sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(...
+ fid, ...
+ [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ...
+ lineLength - logFormat{i}.bytes*logFormat{i}.array, ...
+ logFormat{i}.machineformat) ...
+ );
+ offset = offset + logFormat{i}.bytes*logFormat{i}.array;
+ fseek(fid, offset,'bof');
+ end
+
+ % shot the flight time
+ time_us = sysvector.timestamp(end) - sysvector.timestamp(1);
+ time_s = time_us*1e-6;
+ time_m = time_s/60;
+
+ % close the logfile
+ fclose(fid);
+
+ disp(['end log2matlab conversion' char(10)]);
+ else
+ disp(['file: ' filePath ' does not exist' char(10)]);
+ end
+end
+
+%% ************************************************************************
+% INITCONTROLGUI (nested function)
+% ************************************************************************
+%Setup central control GUI components to control current time where data is shown
+function InitControlGUI()
+ %**********************************************************************
+ % GUI size definitions
+ %**********************************************************************
+ dxy=5; %margins
+ %Panel: Plotctrl
+ dlabels=120;
+ dsliders=200;
+ dedits=80;
+ hslider=20;
+
+ hpanel1=40; %panel1
+ hpanel2=220;%panel2
+ hpanel3=3*hslider+4*dxy+3*dxy;%panel3.
+
+ width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width
+ height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height
+
+ %**********************************************************************
+ % Create GUI
+ %**********************************************************************
+ h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI');
+ h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1));
+ h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1));
+ h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1));
+
+ %%Control GUI-elements
+ %Slider: Current time
+ h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left');
+ h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],...
+ 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel);
+ temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min');
+ set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]);
+ h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),...
+ 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel);
+
+ %Slider: MaxTime
+ h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left');
+ h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],...
+ 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
+ h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),...
+ 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
+
+ %Slider: MinTime
+ h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left');
+ h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],...
+ 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
+ h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),...
+ 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel);
+
+ %%Current data/state GUI-elements (Multiline-edit-box)
+ h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',...
+ 'HorizontalAlignment','left','parent',h.aircraftstatepanel);
+
+ h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',...
+ 'HorizontalAlignment','left','parent',h.guistatepanel);
+
+end
+
+%% ************************************************************************
+% INITPLOTGUI (nested function)
+% ************************************************************************
+function InitPlotGUI()
+
+ % Setup handles to lines and text
+ h.markertext=[];
+ templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array
+ h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively
+ h.markerline(1:NrAxes)=0.0;
+
+ % Setup all other figures and axes for plotting
+ % PLOT WINDOW 1: GPS POSITION
+ h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position');
+ h.axes(1)=axes();
+ set(h.axes(1),'Parent',h.figures(2));
+
+ % PLOT WINDOW 2: IMU, baro altitude
+ h.figures(3)=figure('Name', 'IMU / Baro Altitude');
+ h.axes(2)=subplot(4,1,1);
+ h.axes(3)=subplot(4,1,2);
+ h.axes(4)=subplot(4,1,3);
+ h.axes(5)=subplot(4,1,4);
+ set(h.axes(2:5),'Parent',h.figures(3));
+
+ % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...
+ h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds');
+ h.axes(6)=subplot(4,1,1);
+ h.axes(7)=subplot(4,1,2);
+ h.axes(8)=subplot(4,1,3);
+ h.axes(9)=subplot(4,1,4);
+ set(h.axes(6:9),'Parent',h.figures(4));
+
+ % PLOT WINDOW 4: LOG STATS
+ h.figures(5) = figure('Name', 'Log Statistics');
+ h.axes(10)=subplot(1,1,1);
+ set(h.axes(10:10),'Parent',h.figures(5));
+
+end
+
+%% ************************************************************************
+% DRAWRAWDATA (nested function)
+% ************************************************************************
+%Draws the raw data from the sysvector, but does not add any
+%marker-lines or so
+function DrawRawData()
+ % ************************************************************************
+ % PLOT WINDOW 1: GPS POSITION & GUI
+ % ************************************************************************
+ figure(h.figures(2));
+ % Only plot GPS data if available
+ if (sum(double(sysvector.gps_raw_position(imintime:imaxtime,1)))>0) && (bDisplayGPS)
+ %Draw data
+ plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:imaxtime,1))*fconv_gpslatlong, ...
+ double(sysvector.gps_raw_position(imintime:imaxtime,2))*fconv_gpslatlong, ...
+ double(sysvector.gps_raw_position(imintime:imaxtime,3))*fconv_gpsalt,'r.');
+ title(h.axes(1),'GPS Position Data(if available)');
+ xlabel(h.axes(1),'Latitude [deg]');
+ ylabel(h.axes(1),'Longitude [deg]');
+ zlabel(h.axes(1),'Altitude above MSL [m]');
+ grid on
+
+ %Reset path
+ h.pathpoints=0;
+ end
+
+ % ************************************************************************
+ % PLOT WINDOW 2: IMU, baro altitude
+ % ************************************************************************
+ figure(h.figures(3));
+ plot(h.axes(2),time(imintime:imaxtime),sysvector.mag(imintime:imaxtime,:));
+ title(h.axes(2),'Magnetometers [Gauss]');
+ legend(h.axes(2),'x','y','z');
+ plot(h.axes(3),time(imintime:imaxtime),sysvector.accel(imintime:imaxtime,:));
+ title(h.axes(3),'Accelerometers [m/s²]');
+ legend(h.axes(3),'x','y','z');
+ plot(h.axes(4),time(imintime:imaxtime),sysvector.gyro(imintime:imaxtime,:));
+ title(h.axes(4),'Gyroscopes [rad/s]');
+ legend(h.axes(4),'x','y','z');
+ plot(h.axes(5),time(imintime:imaxtime),sysvector.baro_alt(imintime:imaxtime),'color','blue');
+ if(bDisplayGPS)
+ hold on;
+ plot(h.axes(5),time(imintime:imaxtime),double(sysvector.gps_raw_position(imintime:imaxtime,3)).*fconv_gpsalt,'color','red');
+ hold off
+ legend('Barometric Altitude [m]','GPS Altitude [m]');
+ else
+ legend('Barometric Altitude [m]');
+ end
+ title(h.axes(5),'Altitude above MSL [m]');
+
+ % ************************************************************************
+ % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,...
+ % ************************************************************************
+ figure(h.figures(4));
+ %Attitude Estimate
+ plot(h.axes(6),time(imintime:imaxtime), sysvector.attitude(imintime:imaxtime,:).*180./3.14159);
+ title(h.axes(6),'Estimated attitude [deg]');
+ legend(h.axes(6),'roll','pitch','yaw');
+ %Actuator Controls
+ plot(h.axes(7),time(imintime:imaxtime), sysvector.actuator_control(imintime:imaxtime,:));
+ title(h.axes(7),'Actuator control [-]');
+ legend(h.axes(7),'0','1','2','3');
+ %Actuator Controls
+ plot(h.axes(8),time(imintime:imaxtime), sysvector.actuators(imintime:imaxtime,1:8));
+ title(h.axes(8),'Actuator PWM (raw-)outputs [µs]');
+ legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8');
+ set(h.axes(8), 'YLim',[800 2200]);
+ %Airspeeds
+ plot(h.axes(9),time(imintime:imaxtime), sysvector.ind_airspeed(imintime:imaxtime));
+ hold on
+ plot(h.axes(9),time(imintime:imaxtime), sysvector.true_airspeed(imintime:imaxtime));
+ hold off
+ %add GPS total airspeed here
+ title(h.axes(9),'Airspeed [m/s]');
+ legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed');
+ %calculate time differences and plot them
+ intervals = zeros(0,imaxtime - imintime);
+ for k = imintime+1:imaxtime
+ intervals(k) = time(k) - time(k-1);
+ end
+ plot(h.axes(10), time(imintime:imaxtime), intervals);
+
+ %Set same timescale for all plots
+ for i=2:NrAxes
+ set(h.axes(i),'XLim',[mintime maxtime]);
+ end
+
+ set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);
+end
+
+%% ************************************************************************
+% DRAWCURRENTAIRCRAFTSTATE(nested function)
+% ************************************************************************
+function DrawCurrentAircraftState()
+ %find current data index
+ i=find(time>=CurTime,1,'first');
+
+ %**********************************************************************
+ % Current aircraft state label update
+ %**********************************************************************
+ acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong),'°, ',...
+ 'lon=',num2str(double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong),'°, ',...
+ 'alt=',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]'];
+ acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.mag(i,1)),...
+ ', y=',num2str(sysvector.mag(i,2)),...
+ ', z=',num2str(sysvector.mag(i,3)),']'];
+ acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.accel(i,1)),...
+ ', y=',num2str(sysvector.accel(i,2)),...
+ ', z=',num2str(sysvector.accel(i,3)),']'];
+ acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.gyro(i,1)),...
+ ', y=',num2str(sysvector.gyro(i,2)),...
+ ', z=',num2str(sysvector.gyro(i,3)),']'];
+ acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.baro_alt(i)),'m, GPS: ',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]'];
+ acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.attitude(i,1).*180./3.14159),...
+ ', Pitch=',num2str(sysvector.attitude(i,2).*180./3.14159),...
+ ', Yaw=',num2str(sysvector.attitude(i,3).*180./3.14159),']'];
+ acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:');
+ for j=1:4
+ acstate{7,:}=[acstate{7,:},num2str(sysvector.actuator_control(i,j)),','];
+ end
+ acstate{7,:}=[acstate{7,:},']'];
+ acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:');
+ for j=1:8
+ acstate{8,:}=[acstate{8,:},num2str(sysvector.actuators(i,j)),','];
+ end
+ acstate{8,:}=[acstate{8,:},']'];
+ acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.ind_airspeed(i)),', TAS: ',num2str(sysvector.true_airspeed(i)),']'];
+
+ set(h.edits.AircraftState,'String',acstate);
+
+ %**********************************************************************
+ % GPS Plot Update
+ %**********************************************************************
+ %Plot traveled path, and and time.
+ figure(h.figures(2));
+ hold on;
+ if(CurTime>mintime+1) %the +1 is only a small bugfix
+ h.pathline=plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:i,1))*fconv_gpslatlong, ...
+ double(sysvector.gps_raw_position(imintime:i,2))*fconv_gpslatlong, ...
+ double(sysvector.gps_raw_position(imintime:i,3))*fconv_gpsalt,'b','LineWidth',2);
+ end;
+ hold off
+ %Plot current position
+ newpoint=[double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong double(sysvector.gps_raw_position(i,3))*fconv_gpsalt];
+ if(numel(h.pathpoints)<=3) %empty path
+ h.pathpoints(1,1:3)=newpoint;
+ else %Not empty, append new point
+ h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint;
+ end
+ axes(h.axes(1));
+ line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20);
+
+
+ % Plot current time (small label next to current gps position)
+ textdesc=strcat(' t=',num2str(time(i)),'s');
+ if(isvalidhandle(h.markertext))
+ delete(h.markertext); %delete old text
+ end
+ h.markertext=text(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong,double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong,...
+ double(sysvector.gps_raw_position(i,3))*fconv_gpsalt,textdesc);
+ set(h.edits.CurTime,'String',CurTime);
+
+ %**********************************************************************
+ % Plot the lines showing the current time in the 2-d plots
+ %**********************************************************************
+ for i=2:NrAxes
+ if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end
+ ylims=get(h.axes(i),'YLim');
+ h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black');
+ set(h.markerline(i),'parent',h.axes(i));
+ end
+
+ set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]);
+end
+
+%% ************************************************************************
+% MINMAXTIME CALLBACK (nested function)
+% ************************************************************************
+function minmaxtime_callback(hObj,event) %#ok<INUSL>
+ new_mintime=get(h.sliders.MinTime,'Value');
+ new_maxtime=get(h.sliders.MaxTime,'Value');
+
+ %Safety checks:
+ bErr=false;
+ %1: mintime must be < maxtime
+ if((new_mintime>maxtime) || (new_maxtime<mintime))
+ set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than maxtime! Values were not changed.','BackgroundColor','red');
+ bErr=true;
+ else
+ %2: MinTime must be <=CurTime
+ if(new_mintime>CurTime)
+ set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red');
+ mintime=new_mintime;
+ CurTime=mintime;
+ bErr=true;
+ end
+ %3: MaxTime must be >CurTime
+ if(new_maxtime<CurTime)
+ set(h.labels.GUIState,'String','Error: Maxtime cannot be smaller than CurTime! CurTime set to new maxtime.','BackgroundColor','red');
+ maxtime=new_maxtime;
+ CurTime=maxtime;
+ bErr=true;
+ end
+ end
+
+ if(bErr==false)
+ maxtime=new_maxtime;
+ mintime=new_mintime;
+ end
+
+ %Needs to be done in case values were reset above
+ set(h.sliders.MinTime,'Value',mintime);
+ set(h.sliders.MaxTime,'Value',maxtime);
+
+ %Update curtime-slider
+ set(h.sliders.CurTime,'Value',CurTime);
+ set(h.sliders.CurTime,'Max',maxtime);
+ set(h.sliders.CurTime,'Min',mintime);
+ temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min');
+ set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); %Set Stepsize to constant [in seconds]
+
+ %update edit fields
+ set(h.edits.CurTime,'String',get(h.sliders.CurTime,'Value'));
+ set(h.edits.MinTime,'String',get(h.sliders.MinTime,'Value'));
+ set(h.edits.MaxTime,'String',get(h.sliders.MaxTime,'Value'));
+
+ %Finally, we have to redraw. Update time indices first.
+ [imintime,imaxtime]=FindMinMaxTimeIndices();
+ DrawRawData(); %Rawdata only
+ DrawCurrentAircraftState(); %path info & markers
+end
+
+
+%% ************************************************************************
+% CURTIME CALLBACK (nested function)
+% ************************************************************************
+function curtime_callback(hObj,event) %#ok<INUSL>
+ %find current time
+ if(hObj==h.sliders.CurTime)
+ CurTime=get(h.sliders.CurTime,'Value');
+ elseif (hObj==h.edits.CurTime)
+ temp=str2num(get(h.edits.CurTime,'String'));
+ if(temp<maxtime && temp>mintime)
+ CurTime=temp;
+ else
+ %Error
+ set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red');
+ end
+ else
+ %Error
+ set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red');
+ end
+
+ set(h.sliders.CurTime,'Value',CurTime);
+ set(h.edits.CurTime,'String',num2str(CurTime));
+
+ %Redraw time markers, but don't have to redraw the whole raw data
+ DrawCurrentAircraftState();
+end
+
+%% ************************************************************************
+% FINDMINMAXINDICES (nested function)
+% ************************************************************************
+function [idxmin,idxmax] = FindMinMaxTimeIndices()
+ for i=1:size(sysvector.timestamp,1)
+ if time(i)>=mintime; idxmin=i; break; end
+ end
+ for i=1:size(sysvector.timestamp,1)
+ if maxtime==0; idxmax=size(sysvector.timestamp,1); break; end
+ if time(i)>=maxtime; idxmax=i; break; end
+ end
+ mintime=time(idxmin);
+ maxtime=time(idxmax);
+end
+
+%% ************************************************************************
+% ISVALIDHANDLE (nested function)
+% ************************************************************************
+function isvalid = isvalidhandle(handle)
+ if(exist(varname(handle))>0 && length(ishandle(handle))>0)
+ if(ishandle(handle)>0)
+ if(handle>0.0)
+ isvalid=true;
+ return;
+ end
+ end
+ end
+ isvalid=false;
+end
+
+%% ************************************************************************
+% JUST SOME SMALL HELPER FUNCTIONS (nested function)
+% ************************************************************************
+function out = varname(var)
+ out = inputname(1);
+end
+
+%This is the end of the matlab file / the main function
+end
diff --git a/ROMFS/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
index 75e82bb00..75e82bb00 100644
--- a/ROMFS/mixers/FMU_AERT.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
diff --git a/ROMFS/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/FMU_AET.mix
index 20cb88b91..20cb88b91 100644
--- a/ROMFS/mixers/FMU_AET.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_AET.mix
diff --git a/ROMFS/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix
index ebcb66b24..ebcb66b24 100644
--- a/ROMFS/mixers/FMU_Q.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix
diff --git a/ROMFS/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/FMU_RET.mix
index 95beb8927..95beb8927 100644
--- a/ROMFS/mixers/FMU_RET.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_RET.mix
diff --git a/ROMFS/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix
index 9f81e1dc3..9f81e1dc3 100644
--- a/ROMFS/mixers/FMU_X5.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix
diff --git a/ROMFS/px4fmu_common/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/FMU_delta.mix
new file mode 100644
index 000000000..981466704
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/FMU_delta.mix
@@ -0,0 +1,50 @@
+Delta-wing mixer for PX4FMU
+===========================
+
+This file defines mixers suitable for controlling a delta wing aircraft using
+PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
+servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
+assumed to be unused.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch) and 3 (thrust).
+
+See the README for more information on the scaler format.
+
+Elevon mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+On the assumption that the two elevon servos are physically reversed, the pitch
+input is inverted between the two servos.
+
+The scaling factor for roll inputs is adjusted to implement differential travel
+for the elevons.
+
+M: 2
+O: 10000 10000 0 -10000 10000
+S: 0 0 3000 5000 0 -10000 10000
+S: 0 1 5000 5000 0 -10000 10000
+
+M: 2
+O: 10000 10000 0 -10000 10000
+S: 0 0 5000 3000 0 -10000 10000
+S: 0 1 -5000 -5000 0 -10000 10000
+
+Output 2
+--------
+This mixer is empty.
+
+Z:
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range. Inputs below zero are treated as zero.
+
+M: 1
+O: 10000 10000 0 -10000 10000
+S: 0 3 0 20000 -10000 -10000 10000
+
diff --git a/ROMFS/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
index b5e38ce9e..b5e38ce9e 100644
--- a/ROMFS/mixers/FMU_hex_+.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
diff --git a/ROMFS/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
index 8e8d122ad..8e8d122ad 100644
--- a/ROMFS/mixers/FMU_hex_x.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
diff --git a/ROMFS/mixers/FMU_octo_+.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix
index 2cb70e814..2cb70e814 100644
--- a/ROMFS/mixers/FMU_octo_+.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix
diff --git a/ROMFS/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix
index edc71f013..edc71f013 100644
--- a/ROMFS/mixers/FMU_octo_x.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix
diff --git a/ROMFS/mixers/FMU_pass.mix b/ROMFS/px4fmu_common/mixers/FMU_pass.mix
index e9a81f2bb..e9a81f2bb 100644
--- a/ROMFS/mixers/FMU_pass.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_pass.mix
diff --git a/ROMFS/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
index dfdf1d58e..dfdf1d58e 100644
--- a/ROMFS/mixers/FMU_quad_+.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
new file mode 100644
index 000000000..2a4a0f341
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a quadrotor in the V configuration. All controls
+are mixed 100%.
+
+R: 4v 10000 10000 10000 0
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
new file mode 100644
index 000000000..81b4af30b
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
@@ -0,0 +1,6 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a quadrotor with a wide configuration. All controls are mixed 100%.
+
+R: 4w 10000 10000 10000 0
diff --git a/ROMFS/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
index 12a3bee20..12a3bee20 100644
--- a/ROMFS/mixers/FMU_quad_x.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
diff --git a/ROMFS/mixers/README b/ROMFS/px4fmu_common/mixers/README
index 6649c53c2..6649c53c2 100644
--- a/ROMFS/mixers/README
+++ b/ROMFS/px4fmu_common/mixers/README
diff --git a/ROMFS/scripts/rc.PX4IO b/ROMFS/scripts/rc.PX4IO
deleted file mode 100644
index 1e3963b9a..000000000
--- a/ROMFS/scripts/rc.PX4IO
+++ /dev/null
@@ -1,80 +0,0 @@
-#!nsh
-
-# Disable USB and autostart
-set USB no
-set MODE camflyer
-
-#
-# Start the ORB
-#
-uorb start
-
-#
-# Load microSD params
-#
-echo "[init] loading microSD params"
-param select /fs/microsd/parameters
-if [ -f /fs/microsd/parameters ]
-then
- param load /fs/microsd/parameters
-fi
-
-#
-# Force some key parameters to sane values
-# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
-# see https://pixhawk.ethz.ch/mavlink/
-#
-param set MAV_TYPE 1
-
-#
-# Start the sensors.
-#
-sh /etc/init.d/rc.sensors
-
-#
-# Start MAVLink
-#
-mavlink start -d /dev/ttyS1 -b 57600
-usleep 5000
-
-#
-# Start the commander.
-#
-commander start
-
-#
-# Start GPS interface
-#
-gps start
-
-#
-# Start the attitude estimator
-#
-kalman_demo start
-
-#
-# Start PX4IO interface
-#
-px4io start
-
-#
-# Load mixer and start controllers
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
-control_demo start
-
-#
-# Start logging
-#
-sdlog start -s 10
-
-#
-# Start system state
-#
-if blinkm start
-then
- echo "using BlinkM for state indication"
- blinkm systemstate
-else
- echo "no BlinkM found, OK."
-fi
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index cce388d71..d2ebf1698 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -41,20 +41,19 @@
# The uploader uses the following fields from the firmware file:
#
# image
-# The firmware that will be uploaded.
+# The firmware that will be uploaded.
# image_size
-# The size of the firmware in bytes.
+# The size of the firmware in bytes.
# board_id
-# The board for which the firmware is intended.
+# The board for which the firmware is intended.
# board_revision
-# Currently only used for informational purposes.
+# Currently only used for informational purposes.
#
import sys
import argparse
import binascii
import serial
-import os
import struct
import json
import zlib
@@ -64,292 +63,294 @@ import array
from sys import platform as _platform
+
class firmware(object):
- '''Loads a firmware file'''
-
- desc = {}
- image = bytes()
- crctab = array.array('I', [
- 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
- 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
- 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
- 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
- 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
- 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
- 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
- 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
- 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
- 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
- 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
- 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
- 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
- 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
- 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
- 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
- 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
- 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
- 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
- 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
- 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
- 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
- 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
- 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
- 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
- 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
- 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
- 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
- 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
- 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
- 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
- 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d ])
- crcpad = bytearray('\xff\xff\xff\xff')
-
- def __init__(self, path):
-
- # read the file
- f = open(path, "r")
- self.desc = json.load(f)
- f.close()
-
- self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image'])))
-
- # pad image to 4-byte length
- while ((len(self.image) % 4) != 0):
- self.image.append('\xff')
-
- def property(self, propname):
- return self.desc[propname]
-
- def __crc32(self, bytes, state):
- for byte in bytes:
- index = (state ^ byte) & 0xff
- state = self.crctab[index] ^ (state >> 8)
- return state
-
- def crc(self, padlen):
- state = self.__crc32(self.image, int(0))
- for i in range(len(self.image), (padlen - 1), 4):
- state = self.__crc32(self.crcpad, state)
- return state
+ '''Loads a firmware file'''
+
+ desc = {}
+ image = bytes()
+ crctab = array.array('I', [
+ 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+ 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+ 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+ 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+ 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+ 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+ 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+ 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+ 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+ 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+ 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+ 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+ 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+ 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+ 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+ 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+ 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+ 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+ 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+ 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+ 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+ 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+ 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+ 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+ 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+ 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+ 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+ 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+ 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+ 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+ 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+ 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d])
+ crcpad = bytearray('\xff\xff\xff\xff')
+
+ def __init__(self, path):
+
+ # read the file
+ f = open(path, "r")
+ self.desc = json.load(f)
+ f.close()
+
+ self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image'])))
+
+ # pad image to 4-byte length
+ while ((len(self.image) % 4) != 0):
+ self.image.append('\xff')
+
+ def property(self, propname):
+ return self.desc[propname]
+
+ def __crc32(self, bytes, state):
+ for byte in bytes:
+ index = (state ^ byte) & 0xff
+ state = self.crctab[index] ^ (state >> 8)
+ return state
+
+ def crc(self, padlen):
+ state = self.__crc32(self.image, int(0))
+ for i in range(len(self.image), (padlen - 1), 4):
+ state = self.__crc32(self.crcpad, state)
+ return state
+
class uploader(object):
- '''Uploads a firmware file to the PX FMU bootloader'''
-
- # protocol bytes
- INSYNC = chr(0x12)
- EOC = chr(0x20)
-
- # reply bytes
- OK = chr(0x10)
- FAILED = chr(0x11)
- INVALID = chr(0x13) # rev3+
-
- # command bytes
- NOP = chr(0x00) # guaranteed to be discarded by the bootloader
- GET_SYNC = chr(0x21)
- GET_DEVICE = chr(0x22)
- CHIP_ERASE = chr(0x23)
- CHIP_VERIFY = chr(0x24) # rev2 only
- PROG_MULTI = chr(0x27)
- READ_MULTI = chr(0x28) # rev2 only
- GET_CRC = chr(0x29) # rev3+
- REBOOT = chr(0x30)
-
- INFO_BL_REV = chr(1) # bootloader protocol revision
- BL_REV_MIN = 2 # minimum supported bootloader protocol
- BL_REV_MAX = 3 # maximum supported bootloader protocol
- INFO_BOARD_ID = chr(2) # board type
- INFO_BOARD_REV = chr(3) # board revision
- INFO_FLASH_SIZE = chr(4) # max firmware size in bytes
-
- PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4
- READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64
-
- def __init__(self, portname, baudrate):
- # open the port, keep the default timeout short so we can poll quickly
- self.port = serial.Serial(portname, baudrate, timeout=0.25)
-
- def close(self):
- if self.port is not None:
- self.port.close()
-
- def __send(self, c):
-# print("send " + binascii.hexlify(c))
- self.port.write(str(c))
-
- def __recv(self, count = 1):
- c = self.port.read(count)
- if len(c) < 1:
- raise RuntimeError("timeout waiting for data")
-# print("recv " + binascii.hexlify(c))
- return c
-
- def __recv_int(self):
- raw = self.__recv(4)
- val = struct.unpack("<I", raw)
- return val[0]
-
- def __getSync(self):
- self.port.flush()
- c = self.__recv()
- if c is not self.INSYNC:
- raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c))
- c = self.__recv()
- if c == self.INVALID:
- raise RuntimeError("bootloader reports INVALID OPERATION")
- if c == self.FAILED:
- raise RuntimeError("bootloader reports OPERATION FAILED")
- if c != self.OK:
- raise RuntimeError("unexpected response 0x%x instead of OK" % ord(c))
-
- # attempt to get back into sync with the bootloader
- def __sync(self):
- # send a stream of ignored bytes longer than the longest possible conversation
- # that we might still have in progress
-# self.__send(uploader.NOP * (uploader.PROG_MULTI_MAX + 2))
- self.port.flushInput()
- self.__send(uploader.GET_SYNC
- + uploader.EOC)
- self.__getSync()
-
-# def __trySync(self):
-# c = self.__recv()
-# if (c != self.INSYNC):
-# #print("unexpected 0x%x instead of INSYNC" % ord(c))
-# return False;
-# c = self.__recv()
-# if (c != self.OK):
-# #print("unexpected 0x%x instead of OK" % ord(c))
-# return False
-# return True
-
- # send the GET_DEVICE command and wait for an info parameter
- def __getInfo(self, param):
- self.__send(uploader.GET_DEVICE + param + uploader.EOC)
- value = self.__recv_int()
- self.__getSync()
- return value
-
- # send the CHIP_ERASE command and wait for the bootloader to become ready
- def __erase(self):
- self.__send(uploader.CHIP_ERASE
- + uploader.EOC)
- # erase is very slow, give it 10s
- deadline = time.time() + 10
- while time.time() < deadline:
- try:
- self.__getSync()
- return
- except RuntimeError as ex:
- # we timed out, that's OK
- continue
-
- raise RuntimeError("timed out waiting for erase")
-
- # send a PROG_MULTI command to write a collection of bytes
- def __program_multi(self, data):
- self.__send(uploader.PROG_MULTI
- + chr(len(data)))
- self.__send(data)
- self.__send(uploader.EOC)
- self.__getSync()
-
- # verify multiple bytes in flash
- def __verify_multi(self, data):
- self.__send(uploader.READ_MULTI
- + chr(len(data))
- + uploader.EOC)
- self.port.flush()
- programmed = self.__recv(len(data))
- if programmed != data:
- print(("got " + binascii.hexlify(programmed)))
- print(("expect " + binascii.hexlify(data)))
- return False
- self.__getSync()
- return True
-
- # send the reboot command
- def __reboot(self):
- self.__send(uploader.REBOOT
- + uploader.EOC)
- self.port.flush()
-
- # v3+ can report failure if the first word flash fails
- if self.bl_rev >= 3:
- self.__getSync()
-
- # split a sequence into a list of size-constrained pieces
- def __split_len(self, seq, length):
- return [seq[i:i+length] for i in range(0, len(seq), length)]
-
- # upload code
- def __program(self, fw):
- code = fw.image
- groups = self.__split_len(code, uploader.PROG_MULTI_MAX)
- for bytes in groups:
- self.__program_multi(bytes)
-
- # verify code
- def __verify_v2(self, fw):
- self.__send(uploader.CHIP_VERIFY
- + uploader.EOC)
- self.__getSync()
- code = fw.image
- groups = self.__split_len(code, uploader.READ_MULTI_MAX)
- for bytes in groups:
- if (not self.__verify_multi(bytes)):
- raise RuntimeError("Verification failed")
-
- def __verify_v3(self, fw):
- expect_crc = fw.crc(self.fw_maxsize)
- self.__send(uploader.GET_CRC
- + uploader.EOC)
- report_crc = self.__recv_int()
- self.__getSync()
- if report_crc != expect_crc:
- print(("Expected 0x%x" % expect_crc))
- print(("Got 0x%x" % report_crc))
- raise RuntimeError("Program CRC failed")
-
- # get basic data about the board
- def identify(self):
- # make sure we are in sync before starting
- self.__sync()
-
- # get the bootloader protocol ID first
- self.bl_rev = self.__getInfo(uploader.INFO_BL_REV)
- if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX):
- print(("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV))
- raise RuntimeError("Bootloader protocol mismatch")
-
- self.board_type = self.__getInfo(uploader.INFO_BOARD_ID)
- self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV)
- self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE)
-
- # upload the firmware
- def upload(self, fw):
- # Make sure we are doing the right thing
- if self.board_type != fw.property('board_id'):
- raise RuntimeError("Firmware not suitable for this board (run 'make configure_px4fmu && make clean' or 'make configure_px4io && make clean' to reconfigure).")
- if self.fw_maxsize < fw.property('image_size'):
- raise RuntimeError("Firmware image is too large for this board")
-
- print("erase...")
- self.__erase()
-
- print("program...")
- self.__program(fw)
-
- print("verify...")
- if self.bl_rev == 2:
- self.__verify_v2(fw)
- else:
- self.__verify_v3(fw)
-
- print("done, rebooting.")
- self.__reboot()
- self.port.close()
-
+ '''Uploads a firmware file to the PX FMU bootloader'''
+
+ # protocol bytes
+ INSYNC = chr(0x12)
+ EOC = chr(0x20)
+
+ # reply bytes
+ OK = chr(0x10)
+ FAILED = chr(0x11)
+ INVALID = chr(0x13) # rev3+
+
+ # command bytes
+ NOP = chr(0x00) # guaranteed to be discarded by the bootloader
+ GET_SYNC = chr(0x21)
+ GET_DEVICE = chr(0x22)
+ CHIP_ERASE = chr(0x23)
+ CHIP_VERIFY = chr(0x24) # rev2 only
+ PROG_MULTI = chr(0x27)
+ READ_MULTI = chr(0x28) # rev2 only
+ GET_CRC = chr(0x29) # rev3+
+ REBOOT = chr(0x30)
+
+ INFO_BL_REV = chr(1) # bootloader protocol revision
+ BL_REV_MIN = 2 # minimum supported bootloader protocol
+ BL_REV_MAX = 3 # maximum supported bootloader protocol
+ INFO_BOARD_ID = chr(2) # board type
+ INFO_BOARD_REV = chr(3) # board revision
+ INFO_FLASH_SIZE = chr(4) # max firmware size in bytes
+
+ PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4
+ READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64
+
+ def __init__(self, portname, baudrate):
+ # open the port, keep the default timeout short so we can poll quickly
+ self.port = serial.Serial(portname, baudrate, timeout=0.5)
+
+ def close(self):
+ if self.port is not None:
+ self.port.close()
+
+ def __send(self, c):
+# print("send " + binascii.hexlify(c))
+ self.port.write(str(c))
+
+ def __recv(self, count=1):
+ c = self.port.read(count)
+ if len(c) < 1:
+ raise RuntimeError("timeout waiting for data")
+# print("recv " + binascii.hexlify(c))
+ return c
+
+ def __recv_int(self):
+ raw = self.__recv(4)
+ val = struct.unpack("<I", raw)
+ return val[0]
+
+ def __getSync(self):
+ self.port.flush()
+ c = self.__recv()
+ if c is not self.INSYNC:
+ raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c))
+ c = self.__recv()
+ if c == self.INVALID:
+ raise RuntimeError("bootloader reports INVALID OPERATION")
+ if c == self.FAILED:
+ raise RuntimeError("bootloader reports OPERATION FAILED")
+ if c != self.OK:
+ raise RuntimeError("unexpected response 0x%x instead of OK" % ord(c))
+
+ # attempt to get back into sync with the bootloader
+ def __sync(self):
+ # send a stream of ignored bytes longer than the longest possible conversation
+ # that we might still have in progress
+# self.__send(uploader.NOP * (uploader.PROG_MULTI_MAX + 2))
+ self.port.flushInput()
+ self.__send(uploader.GET_SYNC
+ + uploader.EOC)
+ self.__getSync()
+
+# def __trySync(self):
+# c = self.__recv()
+# if (c != self.INSYNC):
+# #print("unexpected 0x%x instead of INSYNC" % ord(c))
+# return False;
+# c = self.__recv()
+# if (c != self.OK):
+# #print("unexpected 0x%x instead of OK" % ord(c))
+# return False
+# return True
+
+ # send the GET_DEVICE command and wait for an info parameter
+ def __getInfo(self, param):
+ self.__send(uploader.GET_DEVICE + param + uploader.EOC)
+ value = self.__recv_int()
+ self.__getSync()
+ return value
+
+ # send the CHIP_ERASE command and wait for the bootloader to become ready
+ def __erase(self):
+ self.__send(uploader.CHIP_ERASE
+ + uploader.EOC)
+ # erase is very slow, give it 20s
+ deadline = time.time() + 20
+ while time.time() < deadline:
+ try:
+ self.__getSync()
+ return
+ except RuntimeError:
+ # we timed out, that's OK
+ continue
+
+ raise RuntimeError("timed out waiting for erase")
+
+ # send a PROG_MULTI command to write a collection of bytes
+ def __program_multi(self, data):
+ self.__send(uploader.PROG_MULTI
+ + chr(len(data)))
+ self.__send(data)
+ self.__send(uploader.EOC)
+ self.__getSync()
+
+ # verify multiple bytes in flash
+ def __verify_multi(self, data):
+ self.__send(uploader.READ_MULTI
+ + chr(len(data))
+ + uploader.EOC)
+ self.port.flush()
+ programmed = self.__recv(len(data))
+ if programmed != data:
+ print("got " + binascii.hexlify(programmed))
+ print("expect " + binascii.hexlify(data))
+ return False
+ self.__getSync()
+ return True
+
+ # send the reboot command
+ def __reboot(self):
+ self.__send(uploader.REBOOT
+ + uploader.EOC)
+ self.port.flush()
+
+ # v3+ can report failure if the first word flash fails
+ if self.bl_rev >= 3:
+ self.__getSync()
+
+ # split a sequence into a list of size-constrained pieces
+ def __split_len(self, seq, length):
+ return [seq[i:i+length] for i in range(0, len(seq), length)]
+
+ # upload code
+ def __program(self, fw):
+ code = fw.image
+ groups = self.__split_len(code, uploader.PROG_MULTI_MAX)
+ for bytes in groups:
+ self.__program_multi(bytes)
+
+ # verify code
+ def __verify_v2(self, fw):
+ self.__send(uploader.CHIP_VERIFY
+ + uploader.EOC)
+ self.__getSync()
+ code = fw.image
+ groups = self.__split_len(code, uploader.READ_MULTI_MAX)
+ for bytes in groups:
+ if (not self.__verify_multi(bytes)):
+ raise RuntimeError("Verification failed")
+
+ def __verify_v3(self, fw):
+ expect_crc = fw.crc(self.fw_maxsize)
+ self.__send(uploader.GET_CRC
+ + uploader.EOC)
+ report_crc = self.__recv_int()
+ self.__getSync()
+ if report_crc != expect_crc:
+ print("Expected 0x%x" % expect_crc)
+ print("Got 0x%x" % report_crc)
+ raise RuntimeError("Program CRC failed")
+
+ # get basic data about the board
+ def identify(self):
+ # make sure we are in sync before starting
+ self.__sync()
+
+ # get the bootloader protocol ID first
+ self.bl_rev = self.__getInfo(uploader.INFO_BL_REV)
+ if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX):
+ print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV)
+ raise RuntimeError("Bootloader protocol mismatch")
+
+ self.board_type = self.__getInfo(uploader.INFO_BOARD_ID)
+ self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV)
+ self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE)
+
+ # upload the firmware
+ def upload(self, fw):
+ # Make sure we are doing the right thing
+ if self.board_type != fw.property('board_id'):
+ raise RuntimeError("Firmware not suitable for this board")
+ if self.fw_maxsize < fw.property('image_size'):
+ raise RuntimeError("Firmware image is too large for this board")
+
+ print("erase...")
+ self.__erase()
+
+ print("program...")
+ self.__program(fw)
+
+ print("verify...")
+ if self.bl_rev == 2:
+ self.__verify_v2(fw)
+ else:
+ self.__verify_v3(fw)
+
+ print("done, rebooting.")
+ self.__reboot()
+ self.port.close()
+
# Parse commandline arguments
parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.")
@@ -360,57 +361,57 @@ args = parser.parse_args()
# Load the firmware file
fw = firmware(args.firmware)
-print(("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))))
+print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))
# Spin waiting for a device to show up
while True:
- for port in args.port.split(","):
-
- #print("Trying %s" % port)
-
- # create an uploader attached to the port
- try:
- if "linux" in _platform:
- # Linux, don't open Mac OS and Win ports
- if not "COM" in port and not "tty.usb" in port:
- up = uploader(port, args.baud)
- elif "darwin" in _platform:
- # OS X, don't open Windows and Linux ports
- if not "COM" in port and not "ACM" in port:
- up = uploader(port, args.baud)
- elif "win" in _platform:
- # Windows, don't open POSIX ports
- if not "/" in port:
- up = uploader(port, args.baud)
- except:
- # open failed, rate-limit our attempts
- time.sleep(0.05)
-
- # and loop to the next port
- continue
-
- # port is open, try talking to it
- try:
- # identify the bootloader
- up.identify()
- print(("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)))
-
- except:
- # most probably a timeout talking to the port, no bootloader
- continue
-
- try:
- # ok, we have a bootloader, try flashing it
- up.upload(fw)
-
- except RuntimeError as ex:
-
- # print the error
- print(("ERROR: %s" % ex.args))
-
- finally:
- # always close the port
- up.close()
-
- # we could loop here if we wanted to wait for more boards...
- sys.exit(0)
+ for port in args.port.split(","):
+
+ #print("Trying %s" % port)
+
+ # create an uploader attached to the port
+ try:
+ if "linux" in _platform:
+ # Linux, don't open Mac OS and Win ports
+ if not "COM" in port and not "tty.usb" in port:
+ up = uploader(port, args.baud)
+ elif "darwin" in _platform:
+ # OS X, don't open Windows and Linux ports
+ if not "COM" in port and not "ACM" in port:
+ up = uploader(port, args.baud)
+ elif "win" in _platform:
+ # Windows, don't open POSIX ports
+ if not "/" in port:
+ up = uploader(port, args.baud)
+ except:
+ # open failed, rate-limit our attempts
+ time.sleep(0.05)
+
+ # and loop to the next port
+ continue
+
+ # port is open, try talking to it
+ try:
+ # identify the bootloader
+ up.identify()
+ print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
+
+ except:
+ # most probably a timeout talking to the port, no bootloader
+ continue
+
+ try:
+ # ok, we have a bootloader, try flashing it
+ up.upload(fw)
+
+ except RuntimeError as ex:
+
+ # print the error
+ print("ERROR: %s" % ex.args)
+
+ finally:
+ # always close the port
+ up.close()
+
+ # we could loop here if we wanted to wait for more boards...
+ sys.exit(0)
diff --git a/apps/drivers/l3gd20/Makefile b/apps/drivers/l3gd20/Makefile
deleted file mode 100644
index 98e2d57c5..000000000
--- a/apps/drivers/l3gd20/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Makefile to build the L3GD20 driver.
-#
-
-APPNAME = l3gd20
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/mpu6000/Makefile b/apps/drivers/mpu6000/Makefile
deleted file mode 100644
index 32df1bdae..000000000
--- a/apps/drivers/mpu6000/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Makefile to build the BMA180 driver.
-#
-
-APPNAME = mpu6000
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 4096
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/stm32/adc/Makefile b/apps/drivers/stm32/adc/Makefile
deleted file mode 100644
index 443bc0623..000000000
--- a/apps/drivers/stm32/adc/Makefile
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# STM32 ADC driver
-#
-
-APPNAME = adc
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/drivers/stm32/tone_alarm/Makefile b/apps/drivers/stm32/tone_alarm/Makefile
deleted file mode 100644
index d2ddf9534..000000000
--- a/apps/drivers/stm32/tone_alarm/Makefile
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Tone alarm driver
-#
-
-APPNAME = tone_alarm
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/examples/Makefile b/apps/examples/Makefile
index 201be955b..6b614b1aa 100644
--- a/apps/examples/Makefile
+++ b/apps/examples/Makefile
@@ -38,7 +38,6 @@
# Sub-directories
SUBDIRS = adc can cdcacm nsh
-SUBDIRS += math_demo control_demo kalman_demo px4_deamon_app
#SUBDIRS = adc buttons can cdcacm composite cxxtest dhcpd discover elf ftpc
#SUBDIRS += ftpd hello helloxx hidkbd igmp json keypadtest lcdrw mm modbus mount
diff --git a/apps/examples/control_demo/Makefile b/apps/examples/control_demo/Makefile
deleted file mode 100644
index 6e40e645f..000000000
--- a/apps/examples/control_demo/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Basic example application
-#
-
-APPNAME = control_demo
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/examples/kalman_demo/Makefile b/apps/examples/kalman_demo/Makefile
deleted file mode 100644
index 99c34d934..000000000
--- a/apps/examples/kalman_demo/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Basic example application
-#
-
-APPNAME = kalman_demo
-PRIORITY = SCHED_PRIORITY_MAX - 30
-STACKSIZE = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/examples/nsh/Makefile b/apps/examples/nsh/Makefile
index c7d212fc2..ad687958f 100644
--- a/apps/examples/nsh/Makefile
+++ b/apps/examples/nsh/Makefile
@@ -64,6 +64,8 @@ ROOTDEPPATH = --dep-path .
VPATH =
+MAXOPTIMIZATION = -Os
+
all: .built
.PHONY: clean depend distclean
diff --git a/apps/examples/px4_deamon_app/Makefile b/apps/examples/px4_deamon_app/Makefile
deleted file mode 100644
index e4c169872..000000000
--- a/apps/examples/px4_deamon_app/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Basic example application
-#
-
-APPNAME = px4_deamon_app
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/examples/px4_mavlink_debug/Makefile b/apps/examples/px4_mavlink_debug/Makefile
deleted file mode 100644
index f59aef16f..000000000
--- a/apps/examples/px4_mavlink_debug/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Basic example application
-#
-
-APPNAME = px4_mavlink_debug
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/examples/px4_simple_app/Makefile b/apps/examples/px4_simple_app/Makefile
deleted file mode 100644
index 102f51fd4..000000000
--- a/apps/examples/px4_simple_app/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Basic example application
-#
-
-APPNAME = px4_simple_app
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/fixedwing_att_control/Makefile b/apps/fixedwing_att_control/Makefile
deleted file mode 100644
index 01465fa9e..000000000
--- a/apps/fixedwing_att_control/Makefile
+++ /dev/null
@@ -1,45 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Fixedwing Control application
-#
-
-APPNAME = fixedwing_att_control
-PRIORITY = SCHED_PRIORITY_MAX - 30
-STACKSIZE = 2048
-
-INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
-
-include $(APPDIR)/mk/app.mk
-
diff --git a/apps/fixedwing_pos_control/Makefile b/apps/fixedwing_pos_control/Makefile
deleted file mode 100644
index bce391f49..000000000
--- a/apps/fixedwing_pos_control/Makefile
+++ /dev/null
@@ -1,45 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Fixedwing Control application
-#
-
-APPNAME = fixedwing_pos_control
-PRIORITY = SCHED_PRIORITY_MAX - 30
-STACKSIZE = 2048
-
-INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
-
-include $(APPDIR)/mk/app.mk
-
diff --git a/apps/hott_telemetry/Makefile b/apps/hott_telemetry/Makefile
deleted file mode 100644
index 8d5faa3b7..000000000
--- a/apps/hott_telemetry/Makefile
+++ /dev/null
@@ -1,45 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Graupner HoTT Telemetry application.
-#
-
-# The following line is required for accessing UARTs directly.
-INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-
-APPNAME = hott_telemetry
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/mavlink/.context b/apps/mavlink/.context
deleted file mode 100644
index e69de29bb..000000000
--- a/apps/mavlink/.context
+++ /dev/null
diff --git a/apps/mavlink_onboard/Makefile b/apps/mavlink_onboard/Makefile
deleted file mode 100644
index 8b1cb9b2c..000000000
--- a/apps/mavlink_onboard/Makefile
+++ /dev/null
@@ -1,44 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Mavlink Application
-#
-
-APPNAME = mavlink_onboard
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-
-INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/mk/app.mk b/apps/mk/app.mk
deleted file mode 100644
index fa4a12cab..000000000
--- a/apps/mk/app.mk
+++ /dev/null
@@ -1,237 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Common Makefile for nsh command modules and utility libraries; should be
-# included by the module-specific Makefile.
-#
-# To build an app that appears as an nsh external command, the caller
-# must define:
-#
-# APPNAME - the name of the application, defaults to the name
-# of the parent directory.
-#
-# If APPNAME is not defined, a utility library is built instead. The library
-# name is normally automatically determined, but it can be overridden by
-# setting:
-#
-# LIBNAME - the name of the library, defaults to the name of the
-# directory
-#
-# The calling makefile may also set:
-#
-# ASRCS - list of assembly source files, defaults to all .S
-# files in the directory
-#
-# CSRCS - list of C source files, defaults to all .c files
-# in the directory
-#
-# CXXSRCS - list of C++ source files, defaults to all .cpp
-# files in the directory
-#
-# INCLUDES - list of directories to be added to the include
-# search path
-#
-# PRIORITY - thread priority for the command (defaults to
-# SCHED_PRIORITY_DEFAULT)
-#
-# STACKSIZE - stack size for the command (defaults to
-# CONFIG_PTHREAD_STACK_DEFAULT)
-#
-# Symbols in the module are private to the module unless deliberately exported
-# using the __EXPORT tag, or DEFAULT_VISIBILITY is set
-#
-
-############################################################################
-# No user-serviceable parts below
-############################################################################
-
-
-############################################################################
-# Work out who included us so we can report decent errors
-#
-THIS_MAKEFILE := $(lastword $(MAKEFILE_LIST))
-ifeq ($(APP_MAKEFILE),)
-APP_MAKEFILE := $(lastword $(filter-out $(THIS_MAKEFILE),$(MAKEFILE_LIST)))
-endif
-
-############################################################################
-# Get configuration
-#
--include $(TOPDIR)/.config
--include $(TOPDIR)/Make.defs
-include $(APPDIR)/Make.defs
-
-############################################################################
-# Sanity-check the information we've been given and set any defaults
-#
-SRCDIR ?= $(dir $(APP_MAKEFILE))
-PRIORITY ?= SCHED_PRIORITY_DEFAULT
-STACKSIZE ?= CONFIG_PTHREAD_STACK_DEFAULT
-
-INCLUDES += $(APPDIR)
-
-ASRCS ?= $(wildcard $(SRCDIR)/*.S)
-CSRCS ?= $(wildcard $(SRCDIR)/*.c)
-CHDRS ?= $(wildcard $(SRCDIR)/*.h)
-CXXSRCS ?= $(wildcard $(SRCDIR)/*.cpp)
-CXXHDRS ?= $(wildcard $(SRCDIR)/*.hpp)
-
-# if APPNAME is not set, this is a library
-ifeq ($(APPNAME),)
-LIBNAME ?= $(lastword $(subst /, ,$(realpath $(SRCDIR))))
-endif
-
-# there has to be a source file
-ifeq ($(ASRCS)$(CSRCS)$(CXXSRCS),)
-$(error $(realpath $(APP_MAKEFILE)): at least one of ASRCS, CSRCS or CXXSRCS must be set)
-endif
-
-# check that C++ is configured if we have C++ source files and we are building
-ifneq ($(CXXSRCS),)
-ifneq ($(CONFIG_HAVE_CXX),y)
-ifeq ($(MAKECMDGOALS),build)
-$(error $(realpath $(APP_MAKEFILE)): cannot set CXXSRCS if CONFIG_HAVE_CXX not set in configuration)
-endif
-endif
-endif
-
-############################################################################
-# Adjust compilation flags to implement EXPORT
-#
-
-ifeq ($(DEFAULT_VISIBILITY),)
-DEFAULT_VISIBILITY = hidden
-else
-DEFAULT_VISIBILITY = default
-endif
-
-CFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(APPDIR)/systemlib/visibility.h
-CXXFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(APPDIR)/systemlib/visibility.h
-
-############################################################################
-# Add extra include directories
-#
-CFLAGS += $(addprefix -I,$(INCLUDES))
-CXXFLAGS += $(addprefix -I,$(INCLUDES))
-
-############################################################################
-# Things we are going to build
-#
-
-SRCS = $(ASRCS) $(CSRCS) $(CXXSRCS)
-AOBJS = $(patsubst %.S,%.o,$(ASRCS))
-COBJS = $(patsubst %.c,%.o,$(CSRCS))
-CXXOBJS = $(patsubst %.cpp,%.o,$(CXXSRCS))
-OBJS = $(AOBJS) $(COBJS) $(CXXOBJS)
-
-# Automatic depdendency generation
-DEPS = $(OBJS:$(OBJEXT)=.d)
-CFLAGS += -MD
-CXXFLAGS += -MD
-
-# The prelinked object that we are ultimately going to build
-ifneq ($(APPNAME),)
-PRELINKOBJ = $(APPNAME).pre.o
-else
-PRELINKOBJ = $(LIBNAME).pre.o
-endif
-
-# The archive the prelinked object will be linked into
-# XXX does WINTOOL ever get set?
-ifeq ($(WINTOOL),y)
- INCDIROPT = -w
- BIN = "$(shell cygpath -w $(APPDIR)/libapps$(LIBEXT))"
-else
- BIN = "$(APPDIR)/libapps$(LIBEXT)"
-endif
-
-############################################################################
-# Rules for building things
-#
-
-all: .built
-.PHONY: clean depend distclean
-
-#
-# Top-level build; add prelinked object to the apps archive
-#
-.built: $(PRELINKOBJ)
- @$(call ARCHIVE, $(BIN), $(PRELINKOBJ))
- @touch $@
-
-#
-# Source dependencies
-#
-depend:
- @exit 0
-
-ifneq ($(APPNAME),)
-#
-# App registration
-#
-context: .context
-.context: $(MAKEFILE_LIST)
- $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main)
- @touch $@
-else
-context:
- @exit 0
-endif
-
-#
-# Object files
-#
-$(PRELINKOBJ): $(OBJS)
- $(call PRELINK, $@, $(OBJS))
-
-$(AOBJS): %.o : %.S $(MAKEFILE_LIST)
- $(call ASSEMBLE, $<, $@)
-
-$(COBJS): %.o : %.c $(MAKEFILE_LIST)
- $(call COMPILE, $<, $@)
-
-$(CXXOBJS): %.o : %.cpp $(MAKEFILE_LIST)
- $(call COMPILEXX, $<, $@)
-
-#
-# Tidying up
-#
-clean:
- @rm -f $(OBJS) $(DEPS) $(PRELINKOBJ) .built
- $(call CLEAN)
-
-distclean: clean
- @rm -f Make.dep .depend
-
--include $(DEPS)
diff --git a/apps/multirotor_pos_control/Makefile b/apps/multirotor_pos_control/Makefile
deleted file mode 100644
index c88c85435..000000000
--- a/apps/multirotor_pos_control/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Makefile to build multirotor position control
-#
-
-APPNAME = multirotor_pos_control
-PRIORITY = SCHED_PRIORITY_MAX - 25
-STACKSIZE = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/nshlib/Makefile b/apps/nshlib/Makefile
index 76cdac40d..4256a1091 100644
--- a/apps/nshlib/Makefile
+++ b/apps/nshlib/Makefile
@@ -107,6 +107,8 @@ endif
ROOTDEPPATH = --dep-path .
VPATH =
+MAXOPTIMIZATION = -Os
+
# Build targets
all: .built
diff --git a/apps/position_estimator/.context b/apps/position_estimator/.context
deleted file mode 100644
index e69de29bb..000000000
--- a/apps/position_estimator/.context
+++ /dev/null
diff --git a/apps/px4/tests/.context b/apps/px4/tests/.context
deleted file mode 100644
index e69de29bb..000000000
--- a/apps/px4/tests/.context
+++ /dev/null
diff --git a/apps/px4/tests/Makefile b/apps/px4/tests/Makefile
deleted file mode 100644
index cb1c3c618..000000000
--- a/apps/px4/tests/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Makefile to build assorted test cases
-#
-
-APPNAME = tests
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 12000
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/sensors/.context b/apps/sensors/.context
deleted file mode 100644
index e69de29bb..000000000
--- a/apps/sensors/.context
+++ /dev/null
diff --git a/apps/system/i2c/Makefile b/apps/system/i2c/Makefile
index 1ed7a2fae..7f0e6005b 100644
--- a/apps/system/i2c/Makefile
+++ b/apps/system/i2c/Makefile
@@ -64,8 +64,9 @@ VPATH =
APPNAME = i2c
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
+MAXOPTIMIZATION = -Os
-# Build targets
+# Build Targets
all: .built
.PHONY: context .depend depend clean distclean
diff --git a/apps/systemcmds/bl_update/Makefile b/apps/systemcmds/bl_update/Makefile
deleted file mode 100644
index 9d0e156f6..000000000
--- a/apps/systemcmds/bl_update/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build the eeprom tool.
-#
-
-APPNAME = bl_update
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 4096
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/boardinfo/.context b/apps/systemcmds/boardinfo/.context
deleted file mode 100644
index e69de29bb..000000000
--- a/apps/systemcmds/boardinfo/.context
+++ /dev/null
diff --git a/apps/systemcmds/boardinfo/Makefile b/apps/systemcmds/boardinfo/Makefile
deleted file mode 100644
index 753a6843e..000000000
--- a/apps/systemcmds/boardinfo/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build the boardinfo tool.
-#
-
-APPNAME = boardinfo
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/calibration/Makefile b/apps/systemcmds/calibration/Makefile
deleted file mode 100644
index aa1aa7761..000000000
--- a/apps/systemcmds/calibration/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Makefile to build the calibration tool
-#
-
-APPNAME = calibration
-PRIORITY = SCHED_PRIORITY_MAX - 1
-STACKSIZE = 4096
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/calibration/calibration.c b/apps/systemcmds/calibration/calibration.c
deleted file mode 100755
index 7f8b9240f..000000000
--- a/apps/systemcmds/calibration/calibration.c
+++ /dev/null
@@ -1,147 +0,0 @@
-/****************************************************************************
- * calibration.c
- *
- * Copyright (C) 2012 Ivan Ovinnikov. All rights reserved.
- * Authors: Nils Wenzler <wenzlern@ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include "calibration.h"
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-static int calibrate_help(int argc, char *argv[]);
-static int calibrate_all(int argc, char *argv[]);
-
-__EXPORT int calibration_main(int argc, char *argv[]);
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-struct {
- const char *name;
- int (* fn)(int argc, char *argv[]);
- unsigned options;
-#define OPT_NOHELP (1<<0)
-#define OPT_NOALLTEST (1<<1)
-} calibrates[] = {
- {"range", range_cal, 0},
- {"servo", servo_cal, 0},
- {"all", calibrate_all, OPT_NOALLTEST},
- {"help", calibrate_help, OPT_NOALLTEST | OPT_NOHELP},
- {NULL, NULL}
-};
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-static int
-calibrate_help(int argc, char *argv[])
-{
- unsigned i;
-
- printf("Available calibration routines:\n");
-
- for (i = 0; calibrates[i].name; i++)
- printf(" %s\n", calibrates[i].name);
-
- return 0;
-}
-
-static int
-calibrate_all(int argc, char *argv[])
-{
- unsigned i;
- char *args[2] = {"all", NULL};
-
- printf("Running all calibration routines...\n\n");
-
- for (i = 0; calibrates[i].name; i++) {
- printf(" %s:\n", calibrates[i].name);
-
- if (calibrates[i].fn(1, args)) {
- printf(" FAIL\n");
-
- } else {
- printf(" DONE\n");
- }
- }
-
- return 0;
-}
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-void press_enter(void)
-{
- int c;
- printf("Press CTRL+ENTER to continue... \n");
- fflush(stdout);
-
- do c = getchar(); while ((c != '\n') && (c != EOF));
-}
-
-/****************************************************************************
- * Name: calibrate_main
- ****************************************************************************/
-
-int calibration_main(int argc, char *argv[])
-{
- unsigned i;
-
- if (argc < 2) {
- printf("calibration: missing name - 'calibration help' for a list of routines\n");
- return 1;
- }
-
- for (i = 0; calibrates[i].name; i++) {
- if (!strcmp(calibrates[i].name, argv[1]))
- return calibrates[i].fn(argc - 1, argv + 1);
- }
-
- printf("calibrate: no routines called '%s' - 'calibration help' for a list of routines\n", argv[1]);
- return 1;
-}
diff --git a/apps/systemcmds/calibration/channels_cal.c b/apps/systemcmds/calibration/channels_cal.c
deleted file mode 100755
index 575138b12..000000000
--- a/apps/systemcmds/calibration/channels_cal.c
+++ /dev/null
@@ -1,196 +0,0 @@
-/****************************************************************************
- * channels_cal.c
- *
- * Copyright (C) 2012 Nils Wenzler. All rights reserved.
- * Authors: Nils Wenzler <wenzlern@ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include "calibration.h"
-
-/****************************************************************************
- * Defines
- ****************************************************************************/
-#define ABS(a) (((a) < 0) ? -(a) : (a))
-#define MIN(a,b) (((a)<(b))?(a):(b))
-#define MAX(a,b) (((a)>(b))?(a):(b))
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-//Store the values here before writing them to global_data_rc_channels
-uint16_t old_values[NR_CHANNELS];
-uint16_t cur_values[NR_CHANNELS];
-uint8_t function_map[NR_CHANNELS];
-char names[12][9];
-
-
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-void press_enter_2(void)
-{
- int c;
- printf("Press CTRL+ENTER to continue... \n");
- fflush(stdout);
-
- do c = getchar(); while ((c != '\n') && (c != EOF));
-}
-
-/**This gets all current values and writes them to min or max
- */
-uint8_t get_val(uint16_t *buffer)
-{
- if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
- uint8_t i;
-
- for (i = 0; i < NR_CHANNELS; i++) {
- printf("Channel: %i\t RAW Value: %i: \n", i, global_data_rc_channels->chan[i].raw);
- buffer[i] = global_data_rc_channels->chan[i].raw;
- }
-
- global_data_unlock(&global_data_rc_channels->access_conf);
- return 0;
-
- } else
- return -1;
-}
-
-void set_channel(void)
-{
- static uint8_t j = 0;
- uint8_t i;
-
- for (i = 0; i < NR_CHANNELS; i++) {
- if (ABS(old_values - cur_values) > 50) {
- function_map[j] = i;
- strcpy(names[i], global_data_rc_channels->function_names[j]);
- j++;
- }
- }
-}
-
-void write_dat(void)
-{
- global_data_lock(&global_data_rc_channels->access_conf);
- uint8_t i;
-
- for (i = 0; i < NR_CHANNELS; i++) {
- global_data_rc_channels->function[i] = function_map[i];
- strcpy(global_data_rc_channels->chan[i].name, names[i]);
-
- printf("Channel %i\t Function %s\n", i,
- global_data_rc_channels->chan[i].name);
- }
-
- global_data_unlock(&global_data_rc_channels->access_conf);
-}
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-int channels_cal(int argc, char *argv[])
-{
-
- printf("This routine maps the input functions on the remote control\n");
- printf("to the corresponding function (Throttle, Roll,..)\n");
- printf("Always move the stick all the way\n");
- press_enter_2();
-
- printf("Pull the THROTTLE stick down\n");
- press_enter_2();
-
- while (get_val(old_values));
-
- printf("Move the THROTTLE stick up\n ");
- press_enter_2();
-
- while (get_val(cur_values));
-
- set_channel();
-
- printf("Pull the PITCH stick down\n");
- press_enter_2();
-
- while (get_val(old_values));
-
- printf("Move the PITCH stick up\n ");
- press_enter_2();
-
- while (get_val(cur_values));
-
- set_channel();
-
- printf("Put the ROLL stick to the left\n");
- press_enter_2();
-
- while (get_val(old_values));
-
- printf("Put the ROLL stick to the right\n ");
- press_enter_2();
-
- while (get_val(cur_values));
-
- set_channel();
-
- printf("Put the YAW stick to the left\n");
- press_enter_2();
-
- while (get_val(old_values));
-
- printf("Put the YAW stick to the right\n ");
- press_enter_2();
-
- while (get_val(cur_values));
-
- set_channel();
-
- uint8_t k;
-
- for (k = 5; k < NR_CHANNELS; k++) {
- function_map[k] = k;
- strcpy(names[k], global_data_rc_channels->function_names[k]);
- }
-
-//write the values to global_data_rc_channels
- write_dat();
-
- return 0;
-
-}
-
diff --git a/apps/systemcmds/calibration/range_cal.c b/apps/systemcmds/calibration/range_cal.c
deleted file mode 100755
index 159a0d06b..000000000
--- a/apps/systemcmds/calibration/range_cal.c
+++ /dev/null
@@ -1,224 +0,0 @@
-/****************************************************************************
- * range_cal.c
- *
- * Copyright (C) 2012 Nils Wenzler. All rights reserved.
- * Authors: Nils Wenzler <wenzlern@ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-#include <stdio.h>
-#include <stdlib.h>
-#include "calibration.h"
-
-/****************************************************************************
- * Defines
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-//Store the values here before writing them to global_data_rc_channels
-uint16_t max_values[NR_CHANNELS];
-uint16_t min_values[NR_CHANNELS];
-uint16_t mid_values[NR_CHANNELS];
-
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-/**This sets the middle values
- */
-uint8_t set_mid(void)
-{
- if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
- uint8_t i;
-
- for (i = 0; i < NR_CHANNELS; i++) {
- if (i == global_data_rc_channels->function[ROLL] ||
- i == global_data_rc_channels->function[YAW] ||
- i == global_data_rc_channels->function[PITCH]) {
-
- mid_values[i] = global_data_rc_channels->chan[i].raw;
-
- } else {
- mid_values[i] = (max_values[i] + min_values[i]) / 2;
- }
-
- }
-
- global_data_unlock(&global_data_rc_channels->access_conf);
- return 0;
-
- } else
- return -1;
-}
-
-/**This gets all current values and writes them to min or max
- */
-uint8_t get_value(void)
-{
- if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
- uint8_t i;
-
- for (i = 0; i < NR_CHANNELS; i++) {
- //see if the value is bigger or smaller than 1500 (roughly neutral)
- //and write to the appropriate array
- if (global_data_rc_channels->chan[i].raw != 0 &&
- global_data_rc_channels->chan[i].raw < 1500) {
- min_values[i] = global_data_rc_channels->chan[i].raw;
-
- } else if (global_data_rc_channels->chan[i].raw != 0 &&
- global_data_rc_channels->chan[i].raw > 1500) {
- max_values[i] = global_data_rc_channels->chan[i].raw;
-
- } else {
- max_values[i] = 0;
- min_values[i] = 0;
- }
- }
-
- global_data_unlock(&global_data_rc_channels->access_conf);
- return 0;
-
- } else
- return -1;
-}
-
-
-void write_data(void)
-{
- // global_data_lock(&global_data_rc_channels->access_conf);
- // uint8_t i;
- // for(i=0; i < NR_CHANNELS; i++){
- // //Write the data to global_data_rc_channels (if not 0)
- // if(mid_values[i]!=0 && min_values[i]!=0 && max_values[i]!=0){
- // global_data_rc_channels->chan[i].scaling_factor =
- // 10000/((max_values[i] - min_values[i])/2);
- //
- // global_data_rc_channels->chan[i].mid = mid_values[i];
- // }
- //
- // printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
- // i,
- // global_data_rc_channels->function_name[global_data_rc_channels->function[i]],
- // min_values[i], max_values[i],
- // global_data_rc_channels->chan[i].scaling_factor,
- // global_data_rc_channels->chan[i].mid);
- // }
- // global_data_unlock(&global_data_rc_channels->access_conf);
-
- //Write to the Parameter storage
-
- global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN] = min_values[0];
- global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN] = min_values[1];
- global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN] = min_values[2];
- global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN] = min_values[3];
- global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN] = min_values[4];
- global_data_parameter_storage->pm.param_values[PARAM_RC6_MIN] = min_values[5];
- global_data_parameter_storage->pm.param_values[PARAM_RC7_MIN] = min_values[6];
- global_data_parameter_storage->pm.param_values[PARAM_RC8_MIN] = min_values[7];
-
-
- global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] = max_values[0];
- global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] = max_values[1];
- global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] = max_values[2];
- global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] = max_values[3];
- global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] = max_values[4];
- global_data_parameter_storage->pm.param_values[PARAM_RC6_MAX] = max_values[5];
- global_data_parameter_storage->pm.param_values[PARAM_RC7_MAX] = max_values[6];
- global_data_parameter_storage->pm.param_values[PARAM_RC8_MAX] = max_values[7];
-
-
- global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM] = mid_values[0];
- global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM] = mid_values[1];
- global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM] = mid_values[2];
- global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM] = mid_values[3];
- global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM] = mid_values[4];
- global_data_parameter_storage->pm.param_values[PARAM_RC6_TRIM] = mid_values[5];
- global_data_parameter_storage->pm.param_values[PARAM_RC7_TRIM] = mid_values[6];
- global_data_parameter_storage->pm.param_values[PARAM_RC8_TRIM] = mid_values[7];
-
- usleep(3e6);
- uint8_t i;
-
- for (i = 0; i < NR_CHANNELS; i++) {
- printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
- i,
- min_values[i], max_values[i],
- global_data_rc_channels->chan[i].scaling_factor,
- global_data_rc_channels->chan[i].mid);
- }
-
-
-}
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-int range_cal(int argc, char *argv[])
-{
-
- printf("The range calibration routine assumes you already did the channel\n");
- printf("assignment\n");
- printf("This routine chooses the minimum, maximum and middle\n");
- printf("value for each channel separatly. The ROLL, PITCH and YAW function\n");
- printf("get their middle value from the RC direct, for the rest it is\n");
- printf("calculated out of the min and max values.\n");
- press_enter();
-
- printf("Hold both sticks in lower left corner and continue\n ");
- press_enter();
- usleep(500000);
-
- while (get_value());
-
- printf("Hold both sticks in upper right corner and continue\n");
- press_enter();
- usleep(500000);
-
- while (get_value());
-
- printf("Set the trim to 0 and leave the sticks in the neutral position\n");
- press_enter();
-
- //Loop until successfull
- while (set_mid());
-
- //write the values to global_data_rc_channels
- write_data();
-
- return 0;
-
-}
-
diff --git a/apps/systemcmds/calibration/servo_cal.c b/apps/systemcmds/calibration/servo_cal.c
deleted file mode 100644
index 96b3045a9..000000000
--- a/apps/systemcmds/calibration/servo_cal.c
+++ /dev/null
@@ -1,264 +0,0 @@
-/****************************************************************************
- * servo_cal.c
- *
- * Copyright (C) 2012 Nils Wenzler. All rights reserved.
- * Authors: Nils Wenzler <wenzlern@ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-#include <stdio.h>
-#include <stdlib.h>
-#include <arch/board/drv_pwm_servo.h>
-#include <fcntl.h>
-#include "calibration.h"
-
-/****************************************************************************
- * Defines
- ****************************************************************************/
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-//Store the values here before writing them to global_data_rc_channels
-uint16_t max_values_servo[PWM_SERVO_MAX_CHANNELS];
-uint16_t min_values_servo[PWM_SERVO_MAX_CHANNELS];
-uint16_t mid_values_servo[PWM_SERVO_MAX_CHANNELS];
-
-// Servo loop thread
-
-pthread_t servo_calib_thread;
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-/**This sets the middle values
- */
-uint8_t set_mid_s(void)
-{
- if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
- uint8_t i;
-
- for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
- if (i == global_data_rc_channels->function[ROLL] ||
- i == global_data_rc_channels->function[YAW] ||
- i == global_data_rc_channels->function[PITCH]) {
-
- mid_values_servo[i] = global_data_rc_channels->chan[i].raw;
-
- } else {
- mid_values_servo[i] = (max_values_servo[i] + min_values_servo[i]) / 2;
- }
-
- }
-
- global_data_unlock(&global_data_rc_channels->access_conf);
- return 0;
-
- } else
- return -1;
-}
-
-/**This gets all current values and writes them to min or max
- */
-uint8_t get_value_s(void)
-{
- if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
- uint8_t i;
-
- for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
- //see if the value is bigger or smaller than 1500 (roughly neutral)
- //and write to the appropriate array
- if (global_data_rc_channels->chan[i].raw != 0 &&
- global_data_rc_channels->chan[i].raw < 1500) {
- min_values_servo[i] = global_data_rc_channels->chan[i].raw;
-
- } else if (global_data_rc_channels->chan[i].raw != 0 &&
- global_data_rc_channels->chan[i].raw > 1500) {
- max_values_servo[i] = global_data_rc_channels->chan[i].raw;
-
- } else {
- max_values_servo[i] = 0;
- min_values_servo[i] = 0;
- }
- }
-
- global_data_unlock(&global_data_rc_channels->access_conf);
- return 0;
-
- } else
- return -1;
-}
-
-
-void write_data_s(void)
-{
- // global_data_lock(&global_data_rc_channels->access_conf);
- // uint8_t i;
- // for(i=0; i < NR_CHANNELS; i++){
- // //Write the data to global_data_rc_channels (if not 0)
- // if(mid_values_servo[i]!=0 && min_values_servo[i]!=0 && max_values_servo[i]!=0){
- // global_data_rc_channels->chan[i].scaling_factor =
- // 10000/((max_values_servo[i] - min_values_servo[i])/2);
- //
- // global_data_rc_channels->chan[i].mid = mid_values_servo[i];
- // }
- //
- // printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
- // i,
- // global_data_rc_channels->function_name[global_data_rc_channels->function[i]],
- // min_values_servo[i], max_values_servo[i],
- // global_data_rc_channels->chan[i].scaling_factor,
- // global_data_rc_channels->chan[i].mid);
- // }
- // global_data_unlock(&global_data_rc_channels->access_conf);
-
- //Write to the Parameter storage
-
-
-
- global_data_lock(&global_data_parameter_storage->access_conf);
-
- global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MIN] = min_values_servo[0];
- global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MIN] = min_values_servo[1];
- global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MIN] = min_values_servo[2];
- global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MIN] = min_values_servo[3];
-
- global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MAX] = max_values_servo[0];
- global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MAX] = max_values_servo[1];
- global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MAX] = max_values_servo[2];
- global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MAX] = max_values_servo[3];
-
- global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM] = mid_values_servo[0];
- global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM] = mid_values_servo[1];
- global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM] = mid_values_servo[2];
- global_data_parameter_storage->pm.param_values[PARAM_SERVO4_TRIM] = mid_values_servo[3];
-
- global_data_unlock(&global_data_parameter_storage->access_conf);
-
- usleep(3e6);
- uint8_t i;
-
- for (i = 0; i < NR_CHANNELS; i++) {
- printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
- i,
- min_values_servo[i], max_values_servo[i],
- global_data_rc_channels->chan[i].scaling_factor,
- global_data_rc_channels->chan[i].mid);
- }
-
-}
-
-static void *servo_loop(void *arg)
-{
-
- // Set thread name
- prctl(1, "calibration servo", getpid());
-
- // initialize servos
- int fd;
- servo_position_t data[PWM_SERVO_MAX_CHANNELS];
-
- fd = open("/dev/pwm_servo", O_RDWR);
-
- if (fd < 0) {
- printf("failed opening /dev/pwm_servo\n");
- }
-
- ioctl(fd, PWM_SERVO_ARM, 0);
-
- while (1) {
- int i;
-
- for (i = 0; i < 4; i++) {
- data[i] = global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].raw;
- }
-
- int result = write(fd, &data, sizeof(data));
-
- if (result != sizeof(data)) {
- printf("failed bulk-reading channel values\n");
- }
-
- // 5Hz loop
- usleep(200000);
- }
-
- return NULL;
-}
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-int servo_cal(int argc, char *argv[])
-{
- // pthread_attr_t servo_loop_attr;
- // pthread_attr_init(&servo_loop_attr);
- // pthread_attr_setstacksize(&servo_loop_attr, 1024);
- pthread_create(&servo_calib_thread, NULL, servo_loop, NULL);
- pthread_join(servo_calib_thread, NULL);
-
- printf("The servo calibration routine assumes you already did the channel\n");
- printf("assignment with 'calibration channels'\n");
- printf("This routine chooses the minimum, maximum and middle\n");
- printf("value for each channel separately. The ROLL, PITCH and YAW function\n");
- printf("get their middle value from the RC direct, for the rest it is\n");
- printf("calculated out of the min and max values.\n");
- press_enter();
-
- printf("Hold both sticks in lower left corner and continue\n ");
- press_enter();
- usleep(500000);
-
- while (get_value_s());
-
- printf("Hold both sticks in upper right corner and continue\n");
- press_enter();
- usleep(500000);
-
- while (get_value_s());
-
- printf("Set the trim to 0 and leave the sticks in the neutral position\n");
- press_enter();
-
- //Loop until successfull
- while (set_mid_s());
-
- //write the values to global_data_rc_channels
- write_data_s();
-
- return 0;
-
-}
-
diff --git a/apps/systemcmds/delay_test/delay_test.c b/apps/systemcmds/delay_test/delay_test.c
deleted file mode 100644
index 51cce38fc..000000000
--- a/apps/systemcmds/delay_test/delay_test.c
+++ /dev/null
@@ -1,160 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file delay_test.c
- *
- * Simple but effective delay test using leds and a scope to measure
- * communication delays end-to-end accurately.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <poll.h>
-#include <string.h>
-
-#include <systemlib/err.h>
-
-#include <drivers/drv_led.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_tone_alarm.h>
-
-#include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_command.h>
-
-__EXPORT int delay_test_main(int argc, char *argv[]);
-static int led_on(int leds, int led);
-static int led_off(int leds, int led);
-
-int delay_test_main(int argc, char *argv[])
-{
- bool vicon_msg = false;
- bool command_msg = false;
-
- if (argc > 1 && !strcmp(argv[1], "--help")) {
- warnx("usage: delay_test [vicon] [command]\n");
- exit(1);
- }
-
- if (argc > 1 && !strcmp(argv[1], "vicon")) {
- vicon_msg = true;
- }
-
- if (argc > 1 && !strcmp(argv[1], "command")) {
- command_msg = true;
- }
-
- int buzzer = open("/dev/tone_alarm", O_WRONLY);
- int leds = open(LED_DEVICE_PATH, 0);
-
- /* prepare use of amber led */
- led_off(leds, LED_AMBER);
-
- int topic;
-
- /* subscribe to topic */
- if (vicon_msg) {
- topic = orb_subscribe(ORB_ID(vehicle_vicon_position));
- } else if (command_msg) {
- topic = orb_subscribe(ORB_ID(vehicle_command));
- } else {
- errx(1, "No topic selected for delay test, use --help for a list of topics.");
- }
-
- const int testcount = 20;
-
- warnx("running %d iterations..\n", testcount);
-
- struct pollfd fds[1];
- fds[0].fd = topic;
- fds[0].events = POLLIN;
-
- /* display and sound error */
- for (int i = 0; i < testcount; i++)
- {
- /* wait for topic */
- int ret = poll(&fds[0], 1, 2000);
-
- /* this would be bad... */
- if (ret < 0) {
- warnx("poll error %d", errno);
- usleep(1000000);
- continue;
- }
-
- /* do we have a topic update? */
- if (fds[0].revents & POLLIN) {
-
- /* copy object to disable poll ready state */
- if (vicon_msg) {
- struct vehicle_vicon_position_s vicon_position;
- orb_copy(ORB_ID(vehicle_vicon_position), topic, &vicon_position);
- } else if (command_msg) {
- struct vehicle_command_s vehicle_command;
- orb_copy(ORB_ID(vehicle_command), topic, &vehicle_command);
- }
-
- led_on(leds, LED_AMBER);
- ioctl(buzzer, TONE_SET_ALARM, 4);
- /* keep led on for 50 ms to make it barely visible */
- usleep(50000);
- led_off(leds, LED_AMBER);
- }
- }
-
- /* stop alarm */
- ioctl(buzzer, TONE_SET_ALARM, 0);
-
- /* switch on leds */
- led_on(leds, LED_BLUE);
- led_on(leds, LED_AMBER);
-
- exit(0);
-}
-
-static int led_off(int leds, int led)
-{
- return ioctl(leds, LED_OFF, led);
-}
-
-static int led_on(int leds, int led)
-{
- return ioctl(leds, LED_ON, led);
-} \ No newline at end of file
diff --git a/apps/systemcmds/eeprom/Makefile b/apps/systemcmds/eeprom/Makefile
deleted file mode 100644
index 2f3db0fdc..000000000
--- a/apps/systemcmds/eeprom/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Build the eeprom tool.
-#
-
-APPNAME = eeprom
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 4096
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/perf/.context b/apps/systemcmds/perf/.context
deleted file mode 100644
index e69de29bb..000000000
--- a/apps/systemcmds/perf/.context
+++ /dev/null
diff --git a/apps/systemcmds/preflight_check/Makefile b/apps/systemcmds/preflight_check/Makefile
deleted file mode 100644
index f138e2640..000000000
--- a/apps/systemcmds/preflight_check/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Reboot command.
-#
-
-APPNAME = preflight_check
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/reboot/.context b/apps/systemcmds/reboot/.context
deleted file mode 100644
index e69de29bb..000000000
--- a/apps/systemcmds/reboot/.context
+++ /dev/null
diff --git a/apps/systemcmds/reboot/Makefile b/apps/systemcmds/reboot/Makefile
deleted file mode 100644
index 9609a24fd..000000000
--- a/apps/systemcmds/reboot/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Reboot command.
-#
-
-APPNAME = reboot
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/top/.context b/apps/systemcmds/top/.context
deleted file mode 100644
index e69de29bb..000000000
--- a/apps/systemcmds/top/.context
+++ /dev/null
diff --git a/apps/systemcmds/top/Makefile b/apps/systemcmds/top/Makefile
deleted file mode 100644
index c45775f4b..000000000
--- a/apps/systemcmds/top/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# realtime system performance display
-#
-
-APPNAME = top
-PRIORITY = SCHED_PRIORITY_DEFAULT - 10
-STACKSIZE = 3000
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/uORB/.context b/apps/uORB/.context
deleted file mode 100644
index e69de29bb..000000000
--- a/apps/uORB/.context
+++ /dev/null
diff --git a/apps/uORB/Makefile b/apps/uORB/Makefile
deleted file mode 100644
index 64ba52e9c..000000000
--- a/apps/uORB/Makefile
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Makefile to build uORB
-#
-
-APPNAME = uorb
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 4096
-
-include $(APPDIR)/mk/app.mk
diff --git a/makefiles/README.txt b/makefiles/README.txt
new file mode 100644
index 000000000..8b84e4c40
--- /dev/null
+++ b/makefiles/README.txt
@@ -0,0 +1,71 @@
+PX4 Build System
+================
+
+The files in this directory implement the PX4 runtime firmware build system
+and configuration for the standard PX4 boards and software, in conjunction
+with Makefile in the parent directory.
+
+../Makefile
+
+ Top-level makefile for the PX4 build system. This makefile supports
+ building NuttX archives, as well as supervising the building of all
+ of the defined PX4 firmware configurations.
+
+ Try 'make help' in the parent directory for documentation.
+
+firmware.mk
+
+ Manages the build for one specific firmware configuration.
+ See the comments at the top of this file for detailed documentation.
+
+ Builds modules, builtin command lists and the ROMFS (if configured).
+
+ This is the makefile directly used by external build systems; it can
+ be configured to compile modules both inside and outside the PX4
+ source tree. When used in this mode, at least BOARD, MODULES and
+ CONFIG_FILE must be set.
+
+module.mk
+
+ Called by firmware.mk to build individual modules.
+ See the comments at the top of this file for detailed documentation.
+
+ Not normally used other than by firmware.mk.
+
+nuttx.mk
+
+ Called by ../Makefile to build or download the NuttX archives.
+
+upload.mk
+
+ Called by ../Makefile to upload files to a target board. Can be used
+ by external build systems as well.
+
+setup.mk
+
+ Provides common path and tool definitions. Implements host system-specific
+ compatibility hacks.
+
+board_<boardname>.mk
+
+ Board-specific configuration for <boardname>. Typically sets CONFIG_ARCH
+ and then includes the toolchain definition for the board.
+
+config_<boardname>_<configname>.mk
+
+ Parameters for a specific configuration on a specific board.
+ The board name is derived from the filename. Sets MODULES to select
+ source modules to be included in the configuration, may also set
+ ROMFS_ROOT to build a ROMFS and BUILTIN_COMMANDS to include non-module
+ commands (e.g. from NuttX)
+
+toolchain_<toolchainname>.mk
+
+ Provides macros used to compile and link source files.
+ Accepts EXTRADEFINES to add additional pre-processor symbol definitions,
+ EXTRACFLAGS, EXTRACXXFLAGS, EXTRAAFLAGS and EXTRALDFLAGS to pass
+ additional flags to the C compiler, C++ compiler, assembler and linker
+ respectively.
+
+ Defines the COMPILE, COMPILEXX, ASSEMBLE, PRELINK, ARCHIVE and LINK
+ macros that are used elsewhere in the build system.
diff --git a/makefiles/board_px4fmu.mk b/makefiles/board_px4fmu.mk
new file mode 100644
index 000000000..837069644
--- /dev/null
+++ b/makefiles/board_px4fmu.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific definitions for the PX4FMU
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH = CORTEXM4F
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/board_px4io.mk b/makefiles/board_px4io.mk
new file mode 100644
index 000000000..b0eb2dae7
--- /dev/null
+++ b/makefiles/board_px4io.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific definitions for the PX4IO
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH = CORTEXM3
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk
new file mode 100644
index 000000000..ae62b7034
--- /dev/null
+++ b/makefiles/config_px4fmu_default.mk
@@ -0,0 +1,127 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS.
+#
+ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common
+
+#
+# Board support modules
+#
+MODULES += drivers/device
+MODULES += drivers/stm32
+MODULES += drivers/stm32/adc
+MODULES += drivers/stm32/tone_alarm
+MODULES += drivers/led
+MODULES += drivers/px4io
+MODULES += drivers/px4fmu
+MODULES += drivers/boards/px4fmu
+MODULES += drivers/ardrone_interface
+MODULES += drivers/l3gd20
+MODULES += drivers/bma180
+MODULES += drivers/mpu6000
+MODULES += drivers/hmc5883
+MODULES += drivers/ms5611
+MODULES += drivers/mb12xx
+MODULES += drivers/gps
+MODULES += drivers/hil
+MODULES += drivers/hott_telemetry
+MODULES += drivers/blinkm
+MODULES += drivers/mkblctrl
+MODULES += drivers/md25
+MODULES += drivers/ets_airspeed
+MODULES += modules/sensors
+
+#
+# System commands
+#
+MODULES += systemcmds/eeprom
+MODULES += systemcmds/bl_update
+MODULES += systemcmds/boardinfo
+MODULES += systemcmds/i2c
+MODULES += systemcmds/mixer
+MODULES += systemcmds/param
+MODULES += systemcmds/perf
+MODULES += systemcmds/preflight_check
+MODULES += systemcmds/pwm
+MODULES += systemcmds/reboot
+MODULES += systemcmds/top
+MODULES += systemcmds/tests
+
+#
+# General system control
+#
+MODULES += modules/commander
+MODULES += modules/mavlink
+MODULES += modules/mavlink_onboard
+
+#
+# Estimation modules (EKF / other filters)
+#
+MODULES += modules/attitude_estimator_ekf
+MODULES += modules/position_estimator_mc
+MODULES += modules/position_estimator
+MODULES += modules/att_pos_estimator_ekf
+
+#
+# Vehicle Control
+#
+MODULES += modules/fixedwing_backside
+MODULES += modules/fixedwing_att_control
+MODULES += modules/fixedwing_pos_control
+MODULES += modules/multirotor_att_control
+MODULES += modules/multirotor_pos_control
+
+#
+# Logging
+#
+MODULES += modules/sdlog
+
+#
+# Libraries
+#
+MODULES += modules/systemlib
+MODULES += modules/systemlib/mixer
+MODULES += modules/mathlib
+MODULES += modules/mathlib/CMSIS
+MODULES += modules/controllib
+MODULES += modules/uORB
+
+#
+# Demo apps
+#
+#MODULES += examples/math_demo
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/hello_sky
+#MODULES += examples/px4_simple_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/daemon
+#MODULES += examples/px4_daemon_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/debug_values
+#MODULES += examples/px4_mavlink_debug
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+MODULES += examples/fixedwing_control
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+ $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+# command priority stack entrypoint
+BUILTIN_COMMANDS := \
+ $(call _B, sercon, , 2048, sercon_main ) \
+ $(call _B, serdis, , 2048, serdis_main )
diff --git a/makefiles/config_px4io_default.mk b/makefiles/config_px4io_default.mk
new file mode 100644
index 000000000..cf70391bc
--- /dev/null
+++ b/makefiles/config_px4io_default.mk
@@ -0,0 +1,10 @@
+#
+# Makefile for the px4io_default configuration
+#
+
+#
+# Board support modules
+#
+MODULES += drivers/stm32
+MODULES += drivers/boards/px4io
+MODULES += modules/px4iofirmware \ No newline at end of file
diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk
new file mode 100644
index 000000000..497e79237
--- /dev/null
+++ b/makefiles/firmware.mk
@@ -0,0 +1,450 @@
+#
+# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+#
+# Generic Makefile for PX4 firmware images.
+#
+# Requires:
+#
+# BOARD
+# Must be set to a board name known to the PX4 distribution (as
+# we need a corresponding NuttX export archive to link with).
+#
+# Optional:
+#
+# MODULES
+# Contains a list of module paths or path fragments used
+# to find modules. The names listed here are searched in
+# the following directories:
+# <absolute path>
+# $(MODULE_SEARCH_DIRS)
+# WORK_DIR
+# MODULE_SRC
+# PX4_MODULE_SRC
+#
+# Application directories are expected to contain a module.mk
+# file which provides build configuration for the module. See
+# makefiles/module.mk for more details.
+#
+# BUILTIN_COMMANDS
+# Contains a list of built-in commands not explicitly provided
+# by modules / libraries. Each entry in this list is formatted
+# as <command>.<priority>.<stacksize>.<entrypoint>
+#
+# PX4_BASE:
+# Points to a PX4 distribution. Normally determined based on the
+# path to this file.
+#
+# CONFIG:
+# Used when searching for the configuration file, and available
+# to module Makefiles to select optional features.
+# If not set, CONFIG_FILE must be set and CONFIG will be derived
+# automatically from it.
+#
+# CONFIG_FILE:
+# If set, overrides the configuration file search logic. Sets
+# CONFIG to the name of the configuration file, strips any
+# leading config_ prefix and any suffix. e.g. config_board_foo.mk
+# results in CONFIG being set to 'board_foo'.
+#
+# WORK_DIR:
+# Sets the directory in which the firmware will be built. Defaults
+# to the directory 'build' under the directory containing the
+# parent Makefile.
+#
+# ROMFS_ROOT:
+# If set to the path to a directory, a ROMFS image will be generated
+# containing the files under the directory and linked into the final
+# image.
+#
+# MODULE_SEARCH_DIRS:
+# Extra directories to search first for MODULES before looking in the
+# usual places.
+#
+
+################################################################################
+# Paths and configuration
+################################################################################
+
+#
+# Work out where this file is, so we can find other makefiles in the
+# same directory.
+#
+# If PX4_BASE wasn't set previously, work out what it should be
+# and set it here now.
+#
+MK_DIR ?= $(dir $(lastword $(MAKEFILE_LIST)))
+ifeq ($(PX4_BASE),)
+export PX4_BASE := $(abspath $(MK_DIR)/..)
+endif
+$(info % PX4_BASE = $(PX4_BASE))
+ifneq ($(words $(PX4_BASE)),1)
+$(error Cannot build when the PX4_BASE path contains one or more space characters.)
+endif
+
+#
+# Set a default target so that included makefiles or errors here don't
+# cause confusion.
+#
+# XXX We could do something cute here with $(DEFAULT_GOAL) if it's not one
+# of the maintenance targets and set CONFIG based on it.
+#
+all: firmware
+
+#
+# Get path and tool config
+#
+include $(MK_DIR)/setup.mk
+
+#
+# Locate the configuration file
+#
+ifneq ($(CONFIG_FILE),)
+CONFIG := $(subst config_,,$(basename $(notdir $(CONFIG_FILE))))
+else
+CONFIG_FILE := $(wildcard $(PX4_MK_DIR)/config_$(CONFIG).mk)
+endif
+ifeq ($(CONFIG),)
+$(error Missing configuration name or file (specify with CONFIG=<config>))
+endif
+export CONFIG
+include $(CONFIG_FILE)
+$(info % CONFIG = $(CONFIG))
+
+#
+# Sanity-check the BOARD variable and then get the board config.
+# If BOARD was not set by the configuration, extract it automatically.
+#
+# The board config in turn will fetch the toolchain configuration.
+#
+ifeq ($(BOARD),)
+BOARD := $(firstword $(subst _, ,$(CONFIG)))
+endif
+BOARD_FILE := $(wildcard $(PX4_MK_DIR)/board_$(BOARD).mk)
+ifeq ($(BOARD_FILE),)
+$(error Config $(CONFIG) references board $(BOARD), but no board definition file found)
+endif
+export BOARD
+include $(BOARD_FILE)
+$(info % BOARD = $(BOARD))
+
+#
+# If WORK_DIR is not set, create a 'build' directory next to the
+# parent Makefile.
+#
+PARENT_MAKEFILE := $(lastword $(filter-out $(lastword $(MAKEFILE_LIST)),$(MAKEFILE_LIST)))
+ifeq ($(WORK_DIR),)
+export WORK_DIR := $(dir $(PARENT_MAKEFILE))build/
+endif
+$(info % WORK_DIR = $(WORK_DIR))
+
+#
+# Things that, if they change, might affect everything
+#
+GLOBAL_DEPS += $(MAKEFILE_LIST)
+
+#
+# Extra things we should clean
+#
+EXTRA_CLEANS =
+
+################################################################################
+# Modules
+################################################################################
+
+#
+# We don't actually know what a module is called; all we have is a path fragment
+# that we can search for, and where we expect to find a module.mk file.
+#
+# As such, we replicate the successfully-found path inside WORK_DIR for the
+# module's build products in order to keep modules separated from each other.
+#
+# XXX If this becomes unwieldy or breaks for other reasons, we will need to
+# move to allocating directory names and keeping tabs on makefiles via
+# the directory name. That will involve arithmetic (it'd probably be time
+# for GMSL).
+
+# where to look for modules
+MODULE_SEARCH_DIRS += $(WORK_DIR) $(MODULE_SRC) $(PX4_MODULE_SRC)
+
+# sort and unique the modules list
+MODULES := $(sort $(MODULES))
+
+# locate the first instance of a module by full path or by looking on the
+# module search path
+define MODULE_SEARCH
+ $(firstword $(abspath $(wildcard $(1)/module.mk)) \
+ $(abspath $(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk))) \
+ MISSING_$1)
+endef
+
+# make a list of module makefiles and check that we found them all
+MODULE_MKFILES := $(foreach module,$(MODULES),$(call MODULE_SEARCH,$(module)))
+MISSING_MODULES := $(subst MISSING_,,$(filter MISSING_%,$(MODULE_MKFILES)))
+ifneq ($(MISSING_MODULES),)
+$(error Can't find module(s): $(MISSING_MODULES))
+endif
+
+# Make a list of the object files we expect to build from modules
+# Note that this path will typically contain a double-slash at the WORK_DIR boundary; this must be
+# preserved as it is used below to get the absolute path for the module.mk file correct.
+#
+MODULE_OBJS := $(foreach path,$(dir $(MODULE_MKFILES)),$(WORK_DIR)$(path)module.pre.o)
+
+# rules to build module objects
+.PHONY: $(MODULE_OBJS)
+$(MODULE_OBJS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
+$(MODULE_OBJS): mkfile = $(patsubst %module.pre.o,%module.mk,$(relpath))
+$(MODULE_OBJS): workdir = $(@D)
+$(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
+ $(Q) $(MKDIR) -p $(workdir)
+ $(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \
+ -C $(workdir) \
+ MODULE_WORK_DIR=$(workdir) \
+ MODULE_OBJ=$@ \
+ MODULE_MK=$(mkfile) \
+ MODULE_NAME=$(lastword $(subst /, ,$(workdir))) \
+ module
+
+# make a list of phony clean targets for modules
+MODULE_CLEANS := $(foreach path,$(dir $(MODULE_MKFILES)),$(WORK_DIR)$(path)/clean)
+
+# rules to clean modules
+.PHONY: $(MODULE_CLEANS)
+$(MODULE_CLEANS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
+$(MODULE_CLEANS): mkfile = $(patsubst %clean,%module.mk,$(relpath))
+$(MODULE_CLEANS):
+ @$(ECHO) %% cleaning using $(mkfile)
+ $(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \
+ MODULE_WORK_DIR=$(dir $@) \
+ MODULE_MK=$(mkfile) \
+ clean
+
+################################################################################
+# NuttX libraries and paths
+################################################################################
+
+include $(PX4_MK_DIR)/nuttx.mk
+
+################################################################################
+# ROMFS generation
+################################################################################
+
+ifneq ($(ROMFS_ROOT),)
+ifeq ($(wildcard $(ROMFS_ROOT)),)
+$(error ROMFS_ROOT specifies a directory that does not exist)
+endif
+
+#
+# Note that there is no support for more than one root directory or constructing
+# a root from several templates. That would be a nice feature.
+#
+
+# Add dependencies on anything in the ROMFS root
+ROMFS_FILES += $(wildcard \
+ $(ROMFS_ROOT)/* \
+ $(ROMFS_ROOT)/*/* \
+ $(ROMFS_ROOT)/*/*/* \
+ $(ROMFS_ROOT)/*/*/*/* \
+ $(ROMFS_ROOT)/*/*/*/*/* \
+ $(ROMFS_ROOT)/*/*/*/*/*/*)
+ifeq ($(ROMFS_FILES),)
+$(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files)
+endif
+ROMFS_DEPS += $(ROMFS_FILES)
+ROMFS_IMG = romfs.img
+ROMFS_CSRC = $(ROMFS_IMG:.img=.c)
+ROMFS_OBJ = $(ROMFS_CSRC:.c=.o)
+LIBS += $(ROMFS_OBJ)
+LINK_DEPS += $(ROMFS_OBJ)
+
+# Turn the ROMFS image into an object file
+$(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS)
+ $(call BIN_TO_OBJ,$<,$@,romfs_img)
+
+# Generate the ROMFS image from the root
+$(ROMFS_IMG): $(ROMFS_DEPS) $(GLOBAL_DEPS)
+ @$(ECHO) "ROMFS: $@"
+ $(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol"
+
+EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG)
+
+endif
+
+################################################################################
+# Builtin command list generation
+################################################################################
+
+#
+# Builtin commands can be generated by the configuration, in which case they
+# must refer to commands that already exist, or indirectly generated by modules
+# when they are built.
+#
+# The configuration supplies builtin command information in the BUILTIN_COMMANDS
+# variable. Applications make empty files in $(WORK_DIR)/builtin_commands whose
+# filename contains the same information.
+#
+# In each case, the command information consists of four fields separated with a
+# period. These fields are the command's name, its thread priority, its stack size
+# and the name of the function to call when starting the thread.
+#
+BUILTIN_CSRC = $(WORK_DIR)builtin_commands.c
+
+# command definitions from modules (may be empty at Makefile parsing time...)
+MODULE_COMMANDS = $(subst COMMAND.,,$(notdir $(wildcard $(WORK_DIR)builtin_commands/COMMAND.*)))
+
+# We must have at least one pre-defined builtin command in order to generate
+# any of this.
+#
+ifneq ($(BUILTIN_COMMANDS),)
+
+# (BUILTIN_PROTO,<cmdspec>,<outputfile>)
+define BUILTIN_PROTO
+ $(ECHO) 'extern int $(word 4,$1)(int argc, char *argv[]);' >> $2;
+endef
+
+# (BUILTIN_DEF,<cmdspec>,<outputfile>)
+define BUILTIN_DEF
+ $(ECHO) ' {"$(word 1,$1)", $(word 2,$1), $(word 3,$1), $(word 4,$1)},' >> $2;
+endef
+
+# Don't generate until modules have updated their command files
+$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(BUILTIN_COMMAND_FILES)
+ @$(ECHO) "CMDS: $@"
+ $(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@
+ $(Q) $(ECHO) '#include <nuttx/config.h>' >> $@
+ $(Q) $(ECHO) '#include <nuttx/binfmt/builtin.h>' >> $@
+ $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@))
+ $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_PROTO,$(subst ., ,$(spec)),$@))
+ $(Q) $(ECHO) 'const struct builtin_s g_builtins[] = {' >> $@
+ $(Q) $(foreach spec,$(BUILTIN_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@))
+ $(Q) $(foreach spec,$(MODULE_COMMANDS),$(call BUILTIN_DEF,$(subst ., ,$(spec)),$@))
+ $(Q) $(ECHO) ' {NULL, 0, 0, NULL}' >> $@
+ $(Q) $(ECHO) '};' >> $@
+ $(Q) $(ECHO) 'const int g_builtin_count = $(words $(BUILTIN_COMMANDS) $(MODULE_COMMANDS));' >> $@
+
+SRCS += $(BUILTIN_CSRC)
+
+EXTRA_CLEANS += $(BUILTIN_CSRC)
+
+endif
+
+################################################################################
+# Default SRCS generation
+################################################################################
+
+#
+# If there are no SRCS, the build will fail; in that case, generate an empty
+# source file.
+#
+ifeq ($(SRCS),)
+EMPTY_SRC = $(WORK_DIR)empty.c
+$(EMPTY_SRC):
+ $(Q) $(ECHO) '/* this is an empty file */' > $@
+
+SRCS += $(EMPTY_SRC)
+endif
+
+################################################################################
+# Build rules
+################################################################################
+
+#
+# What we're going to build.
+#
+PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4
+PRODUCT_BIN = $(WORK_DIR)firmware.bin
+PRODUCT_ELF = $(WORK_DIR)firmware.elf
+
+.PHONY: firmware
+firmware: $(PRODUCT_BUNDLE)
+
+#
+# Object files we will generate from sources
+#
+OBJS := $(foreach src,$(SRCS),$(WORK_DIR)$(src).o)
+
+#
+# SRCS -> OBJS rules
+#
+
+$(OBJS): $(GLOBAL_DEPS)
+
+$(filter %.c.o,$(OBJS)): $(WORK_DIR)%.c.o: %.c $(GLOBAL_DEPS)
+ $(call COMPILE,$<,$@)
+
+$(filter %.cpp.o,$(OBJS)): $(WORK_DIR)%.cpp.o: %.cpp $(GLOBAL_DEPS)
+ $(call COMPILEXX,$<,$@)
+
+$(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS)
+ $(call ASSEMBLE,$<,$@)
+
+#
+# Built product rules
+#
+
+$(PRODUCT_BUNDLE): $(PRODUCT_BIN)
+ @$(ECHO) %% Generating $@
+ $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \
+ --git_identity $(PX4_BASE) \
+ --image $< > $@
+
+$(PRODUCT_BIN): $(PRODUCT_ELF)
+ $(call SYM_TO_BIN,$<,$@)
+
+$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES)
+ $(call LINK,$@,$(OBJS) $(MODULE_OBJS))
+
+#
+# Utility rules
+#
+
+.PHONY: upload
+upload: $(PRODUCT_BUNDLE) $(PRODUCT_BIN)
+ $(Q) $(MAKE) -f $(PX4_MK_DIR)/upload.mk \
+ METHOD=serial \
+ CONFIG=$(CONFIG) \
+ BOARD=$(BOARD) \
+ BUNDLE=$(PRODUCT_BUNDLE) \
+ BIN=$(PRODUCT_BIN)
+
+.PHONY: clean
+clean: $(MODULE_CLEANS)
+ @$(ECHO) %% cleaning
+ $(Q) $(REMOVE) $(PRODUCT_BUNDLE) $(PRODUCT_BIN) $(PRODUCT_ELF)
+ $(Q) $(REMOVE) $(OBJS) $(DEP_INCLUDES) $(EXTRA_CLEANS)
+ $(Q) $(RMDIR) $(NUTTX_EXPORT_DIR)
+
+
+#
+# DEP_INCLUDES is defined by the toolchain include in terms of $(OBJS)
+#
+-include $(DEP_INCLUDES)
diff --git a/makefiles/module.mk b/makefiles/module.mk
new file mode 100644
index 000000000..db6f4e15e
--- /dev/null
+++ b/makefiles/module.mk
@@ -0,0 +1,241 @@
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+#
+# Framework makefile for PX4 modules
+#
+# This makefile is invoked by firmware.mk to build each of the modules
+# that will subsequently be linked into the firmware image.
+#
+# Applications are built as prelinked objects with a limited set of exported
+# symbols, as the global namespace is shared between all modules. Normally an
+# module will just export one or more <command>_main functions.
+#
+# IMPORTANT NOTE:
+#
+# This makefile assumes it is being invoked in the module's output directory.
+#
+
+#
+# Variables that can be set by the module's module.mk:
+#
+#
+# SRCS (required)
+#
+# Lists the .c, cpp and .S files that should be compiled/assembled to
+# produce the module.
+#
+# MODULE_COMMAND (optional)
+# MODULE_ENTRYPOINT (optional if MODULE_COMMAND is set)
+# MODULE_STACKSIZE (optional if MODULE_COMMAND is set)
+# MODULE_PRIORITY (optional if MODULE_COMMAND is set)
+#
+# Defines a single builtin command exported by the module.
+# MODULE_COMMAND must be unique for any configuration, but need not be the
+# same as the module directory name.
+#
+# If MODULE_ENTRYPOINT is set, it names the function (which must be exported)
+# that will be the entrypoint for the builtin command. It defaults to
+# $(MODULE_COMMAND)_main.
+#
+# If MODULE_STACKSIZE is set, it is the size in bytes of the stack to be
+# allocated for the builtin command. If it is not set, it defaults
+# to CONFIG_PTHREAD_STACK_DEFAULT.
+#
+# If MODULE_PRIORITY is set, it is the thread priority for the builtin
+# command. If it is not set, it defaults to SCHED_PRIORITY_DEFAULT.
+#
+# MODULE_COMMANDS (optional if MODULE_COMMAND is not set)
+#
+# Defines builtin commands exported by the module. Each word in
+# the list should be formatted as:
+# <command>.<priority>.<stacksize>.<entrypoint>
+#
+# INCLUDE_DIRS (optional, must be appended)
+#
+# The list of directories searched for include files. If non-standard
+# includes (e.g. those from another module) are required, paths to search
+# can be added here.
+#
+# DEFAULT_VISIBILITY (optional)
+#
+# If not set, global symbols defined in a module will not be visible
+# outside the module. Symbols that should be globally visible must be
+# marked __EXPORT.
+# If set, global symbols defined in a module will be globally visible.
+#
+
+#
+# Variables visible to the module's module.mk:
+#
+# CONFIG
+# BOARD
+# MODULE_WORK_DIR
+# MODULE_OBJ
+# MODULE_MK
+# Anything set in setup.mk, board_$(BOARD).mk and the toolchain file.
+# Anything exported from config_$(CONFIG).mk
+#
+
+################################################################################
+# No user-serviceable parts below.
+################################################################################
+
+ifeq ($(MODULE_MK),)
+$(error No module makefile specified)
+endif
+$(info %% MODULE_MK = $(MODULE_MK))
+
+#
+# Get the board/toolchain config
+#
+include $(PX4_MK_DIR)/board_$(BOARD).mk
+
+#
+# Get the module's config
+#
+include $(MODULE_MK)
+MODULE_SRC := $(dir $(MODULE_MK))
+$(info % MODULE_NAME = $(MODULE_NAME))
+$(info % MODULE_SRC = $(MODULE_SRC))
+$(info % MODULE_OBJ = $(MODULE_OBJ))
+$(info % MODULE_WORK_DIR = $(MODULE_WORK_DIR))
+
+#
+# Things that, if they change, might affect everything
+#
+GLOBAL_DEPS += $(MAKEFILE_LIST)
+
+################################################################################
+# Builtin command definitions
+################################################################################
+
+ifneq ($(MODULE_COMMAND),)
+MODULE_ENTRYPOINT ?= $(MODULE_COMMAND)_main
+MODULE_STACKSIZE ?= CONFIG_PTHREAD_STACK_DEFAULT
+MODULE_PRIORITY ?= SCHED_PRIORITY_DEFAULT
+MODULE_COMMANDS += $(MODULE_COMMAND).$(MODULE_PRIORITY).$(MODULE_STACKSIZE).$(MODULE_ENTRYPOINT)
+endif
+
+ifneq ($(MODULE_COMMANDS),)
+MODULE_COMMAND_FILES := $(addprefix $(WORK_DIR)/builtin_commands/COMMAND.,$(MODULE_COMMANDS))
+
+# Create the command files
+# Ensure that there is only one entry for each command
+#
+.PHONY: $(MODULE_COMMAND_FILES)
+$(MODULE_COMMAND_FILES): command = $(word 2,$(subst ., ,$(notdir $(@))))
+$(MODULE_COMMAND_FILES): exclude = $(dir $@)COMMAND.$(command).*
+$(MODULE_COMMAND_FILES): $(GLOBAL_DEPS)
+ @$(REMOVE) -f $(exclude)
+ @$(MKDIR) -p $(dir $@)
+ @$(ECHO) "CMD: $(command)"
+ $(Q) $(TOUCH) $@
+endif
+
+################################################################################
+# Adjust compilation flags to implement EXPORT
+################################################################################
+
+ifeq ($(DEFAULT_VISIBILITY),)
+DEFAULT_VISIBILITY = hidden
+else
+DEFAULT_VISIBILITY = default
+endif
+
+CFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibility.h
+CXXFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibility.h
+
+################################################################################
+# Build rules
+################################################################################
+
+#
+# What we're going to build
+#
+module: $(MODULE_OBJ) $(MODULE_COMMAND_FILES)
+
+##
+## Locate sources (allows relative source paths in module.mk)
+##
+#define SRC_SEARCH
+# $(abspath $(firstword $(wildcard $1) $(wildcard $(MODULE_SRC)/$1) MISSING_$1))
+#endef
+#
+#ABS_SRCS ?= $(foreach src,$(SRCS),$(call SRC_SEARCH,$(src)))
+#MISSING_SRCS := $(subst MISSING_,,$(filter MISSING_%,$(ABS_SRCS)))
+#ifneq ($(MISSING_SRCS),)
+#$(error $(MODULE_MK): missing in SRCS: $(MISSING_SRCS))
+#endif
+#ifeq ($(ABS_SRCS),)
+#$(error $(MODULE_MK): nothing to compile in SRCS)
+#endif
+#
+##
+## Object files we will generate from sources
+##
+#OBJS := $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
+
+OBJS = $(addsuffix .o,$(SRCS))
+$(info SRCS $(SRCS))
+$(info OBJS $(OBJS))
+
+#
+# SRCS -> OBJS rules
+#
+
+$(OBJS): $(GLOBAL_DEPS)
+
+vpath %.c $(MODULE_SRC)
+$(filter %.c.o,$(OBJS)): %.c.o: %.c $(GLOBAL_DEPS)
+ $(call COMPILE,$<,$@)
+
+vpath %.cpp $(MODULE_SRC)
+$(filter %.cpp.o,$(OBJS)): %.cpp.o: %.cpp $(GLOBAL_DEPS)
+ $(call COMPILEXX,$<,$@)
+
+vpath %.S $(MODULE_SRC)
+$(filter %.S.o,$(OBJS)): %.S.o: %.S $(GLOBAL_DEPS)
+ $(call ASSEMBLE,$<,$@)
+
+#
+# Built product rules
+#
+
+$(MODULE_OBJ): $(OBJS) $(GLOBAL_DEPS)
+ $(call PRELINK,$@,$(OBJS))
+
+#
+# Utility rules
+#
+
+clean:
+ $(Q) $(REMOVE) $(MODULE_PRELINK) $(OBJS)
diff --git a/apps/px4io/Makefile b/makefiles/nuttx.mk
index 0eb3ffcf7..346735a02 100644
--- a/apps/px4io/Makefile
+++ b/makefiles/nuttx.mk
@@ -1,4 +1,3 @@
-############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
#
@@ -29,30 +28,51 @@
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
-############################################################################
#
-# Build the px4io application.
+# Rules and definitions related to handling the NuttX export archives when
+# building firmware.
#
#
-# Note that we pull a couple of specific files from the systemlib, since
-# we can't support it all.
+# Check that the NuttX archive for the selected board is available.
#
-CSRCS = adc.c \
- controls.c \
- dsm.c \
- i2c.c \
- px4io.c \
- registers.c \
- safety.c \
- sbus.c \
- ../systemlib/hx_stream.c \
- ../systemlib/perf_counter.c \
- ../systemlib/up_cxxinitialize.c
+NUTTX_ARCHIVE := $(wildcard $(ARCHIVE_DIR)$(BOARD).export)
+ifeq ($(NUTTX_ARCHIVE),)
+$(error The NuttX export archive for $(BOARD) is missing from $(ARCHIVE_DIR) - try 'make archives' in $(PX4_BASE))
+endif
-CXXSRCS = mixer.cpp
+#
+# The NuttX config header should always be present in the NuttX archive, and
+# if it changes, everything should be rebuilt. So, use it as the trigger to
+# unpack the NuttX archive.
+#
+NUTTX_EXPORT_DIR = $(WORK_DIR)nuttx-export/
+NUTTX_CONFIG_HEADER = $(NUTTX_EXPORT_DIR)include/nuttx/config.h
+$(info % NUTTX_EXPORT_DIR = $(NUTTX_EXPORT_DIR))
+$(info % NUTTX_CONFIG_HEADER = $(NUTTX_CONFIG_HEADER))
+
+
+GLOBAL_DEPS += $(NUTTX_CONFIG_HEADER)
+
+#
+# Use the linker script from the NuttX export
+#
+LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script
+
+#
+# Add directories from the NuttX export to the relevant search paths
+#
+INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \
+ $(NUTTX_EXPORT_DIR)arch/chip \
+ $(NUTTX_EXPORT_DIR)arch/common
-INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+LIB_DIRS += $(NUTTX_EXPORT_DIR)libs
+LIBS += -lapps -lnuttx
+LINK_DEPS += $(NUTTX_EXPORT_DIR)libs/libapps.a \
+ $(NUTTX_EXPORT_DIR)libs/libnuttx.a
-include $(APPDIR)/mk/app.mk
+$(NUTTX_CONFIG_HEADER): $(NUTTX_ARCHIVE)
+ @$(ECHO) %% Unpacking $(NUTTX_ARCHIVE)
+ $(Q) $(UNZIP_CMD) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE)
+ $(Q) $(TOUCH) $@
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
new file mode 100644
index 000000000..111193093
--- /dev/null
+++ b/makefiles/setup.mk
@@ -0,0 +1,91 @@
+#
+# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+#
+# Path and tool setup
+#
+
+#
+# Some useful paths.
+#
+# Note that in general we always keep directory paths with the separator
+# at the end, and join paths without explicit separators. This reduces
+# the number of duplicate slashes we have lying around in paths,
+# and is consistent with joining the results of $(dir) and $(notdir).
+#
+export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/
+export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/
+export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/
+export NUTTX_SRC = $(abspath $(PX4_BASE)/nuttx)/
+export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/apps)/
+export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/
+export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/
+export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/
+export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/
+export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/
+
+#
+# Default include paths
+#
+export INCLUDE_DIRS := $(PX4_MODULE_SRC) \
+ $(PX4_MODULE_SRC)/modules/ \
+ $(PX4_INCLUDE_DIR)
+
+#
+# Tools
+#
+export MKFW = $(PX4_BASE)/Tools/px_mkfw.py
+export UPLOADER = $(PX4_BASE)/Tools/px_uploader.py
+export COPY = cp
+export REMOVE = rm -f
+export RMDIR = rm -rf
+export GENROMFS = genromfs
+export TOUCH = touch
+export MKDIR = mkdir
+export ECHO = echo
+export UNZIP_CMD = unzip
+export PYTHON = python
+export OPENOCD = openocd
+
+#
+# Host-specific paths, hacks and fixups
+#
+export SYSTYPE := $(shell uname -s)
+
+ifeq ($(SYSTYPE),Darwin)
+# Eclipse may not have the toolchain on its path.
+export PATH := $(PATH):/usr/local/bin
+endif
+
+#
+# Makefile debugging.
+#
+export Q := $(if $(V),,@)
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
new file mode 100644
index 000000000..874e7154c
--- /dev/null
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -0,0 +1,285 @@
+#
+# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+#
+# Definitions for a generic GNU ARM-EABI toolchain
+#
+
+#$(info TOOLCHAIN gnu-arm-eabi)
+
+# Toolchain commands. Normally only used inside this file.
+#
+CROSSDEV = arm-none-eabi-
+
+CC = $(CROSSDEV)gcc
+CXX = $(CROSSDEV)g++
+CPP = $(CROSSDEV)gcc -E
+LD = $(CROSSDEV)ld
+AR = $(CROSSDEV)ar rcs
+NM = $(CROSSDEV)nm
+OBJCOPY = $(CROSSDEV)objcopy
+OBJDUMP = $(CROSSDEV)objdump
+
+# XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup
+
+MAXOPTIMIZATION = -O3
+
+# Base CPU flags for each of the supported architectures.
+#
+ARCHCPUFLAGS_CORTEXM4F = -mcpu=cortex-m4 \
+ -mthumb \
+ -march=armv7e-m \
+ -mfpu=fpv4-sp-d16 \
+ -mfloat-abi=hard
+
+ARCHCPUFLAGS_CORTEXM4 = -mcpu=cortex-m4 \
+ -mthumb \
+ -march=armv7e-m \
+ -mfloat-abi=soft
+
+ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \
+ -mthumb \
+ -march=armv7-m \
+ -mfloat-abi=soft
+
+# Pick the right set of flags for the architecture.
+#
+ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH))
+ifeq ($(ARCHCPUFLAGS),)
+$(error Must set CONFIG_ARCH to one of CORTEXM4F, CORTEXM4 or CORTEXM3)
+endif
+
+# optimisation flags
+#
+ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
+ -g \
+ -fno-strict-aliasing \
+ -fno-strength-reduce \
+ -fomit-frame-pointer \
+ -funsafe-math-optimizations \
+ -fno-builtin-printf \
+ -ffunction-sections \
+ -fdata-sections
+
+# enable precise stack overflow tracking
+# note - requires corresponding support in NuttX
+INSTRUMENTATIONDEFINES = -finstrument-functions \
+ -ffixed-r10
+# Language-specific flags
+#
+ARCHCFLAGS = -std=gnu99
+ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x
+
+# Generic warnings
+#
+ARCHWARNINGS = -Wall \
+ -Wextra \
+ -Wdouble-promotion \
+ -Wshadow \
+ -Wfloat-equal \
+ -Wframe-larger-than=1024 \
+ -Wpointer-arith \
+ -Wlogical-op \
+ -Wmissing-declarations \
+ -Wpacked \
+ -Wno-unused-parameter
+# -Wcast-qual - generates spurious noreturn attribute warnings, try again later
+# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+# C-specific warnings
+#
+ARCHCWARNINGS = $(ARCHWARNINGS) \
+ -Wbad-function-cast \
+ -Wstrict-prototypes \
+ -Wold-style-declaration \
+ -Wmissing-parameter-type \
+ -Wmissing-prototypes \
+ -Wnested-externs \
+ -Wunsuffixed-float-constants
+
+# C++-specific warnings
+#
+ARCHWARNINGSXX = $(ARCHWARNINGS)
+
+# pull in *just* libm from the toolchain ... this is grody
+LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a)
+EXTRA_LIBS += $(LIBM)
+
+# Flags we pass to the C compiler
+#
+CFLAGS = $(ARCHCFLAGS) \
+ $(ARCHCWARNINGS) \
+ $(ARCHOPTIMIZATION) \
+ $(ARCHCPUFLAGS) \
+ $(ARCHINCLUDES) \
+ $(INSTRUMENTATIONDEFINES) \
+ $(ARCHDEFINES) \
+ $(EXTRADEFINES) \
+ $(EXTRACFLAGS) \
+ -fno-common \
+ $(addprefix -I,$(INCLUDE_DIRS))
+
+# Flags we pass to the C++ compiler
+#
+CXXFLAGS = $(ARCHCXXFLAGS) \
+ $(ARCHWARNINGSXX) \
+ $(ARCHOPTIMIZATION) \
+ $(ARCHCPUFLAGS) \
+ $(ARCHXXINCLUDES) \
+ $(INSTRUMENTATIONDEFINES) \
+ $(ARCHDEFINES) \
+ -DCONFIG_WCHAR_BUILTIN \
+ $(EXTRADEFINES) \
+ $(EXTRACXXFLAGS) \
+ $(addprefix -I,$(INCLUDE_DIRS))
+
+# Flags we pass to the assembler
+#
+AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \
+ $(EXTRADEFINES) \
+ $(EXTRAAFLAGS)
+
+# Flags we pass to the linker
+#
+LDFLAGS += --warn-common \
+ --gc-sections \
+ $(EXTRALDFLAGS) \
+ $(addprefix -T,$(LDSCRIPT)) \
+ $(addprefix -L,$(LIB_DIRS))
+
+# Compiler support library
+#
+LIBGCC := $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name)
+
+# Files that the final link depends on
+#
+LINK_DEPS += $(LDSCRIPT)
+
+# Files to include to get automated dependencies
+#
+DEP_INCLUDES = $(subst .o,.d,$(OBJS))
+
+# Compile C source $1 to object $2
+# as a side-effect, generate a dependency file
+#
+define COMPILE
+ @$(ECHO) "CC: $1"
+ @$(MKDIR) -p $(dir $2)
+ $(Q) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2
+endef
+
+# Compile C++ source $1 to $2
+# as a side-effect, generate a dependency file
+#
+define COMPILEXX
+ @$(ECHO) "CXX: $1"
+ @$(MKDIR) -p $(dir $2)
+ $(Q) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2
+endef
+
+# Assemble $1 into $2
+#
+define ASSEMBLE
+ @$(ECHO) "AS: $1"
+ @$(MKDIR) -p $(dir $2)
+ $(Q) $(CC) -c $(AFLAGS) $(abspath $1) -o $2
+endef
+
+# Produce partially-linked $1 from files in $2
+#
+define PRELINK
+ @$(ECHO) "PRELINK: $1"
+ @$(MKDIR) -p $(dir $1)
+ $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+# Update the archive $1 with the files in $2
+#
+define ARCHIVE
+ @$(ECHO) "AR: $2"
+ @$(MKDIR) -p $(dir $1)
+ $(Q) $(AR) $1 $2
+endef
+
+# Link the objects in $2 into the binary $1
+#
+define LINK
+ @$(ECHO) "LINK: $1"
+ @$(MKDIR) -p $(dir $1)
+ $(Q) $(LD) $(LDFLAGS) -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group
+endef
+
+# Convert $1 from a linked object to a raw binary in $2
+#
+define SYM_TO_BIN
+ @$(ECHO) "BIN: $2"
+ @$(MKDIR) -p $(dir $2)
+ $(Q) $(OBJCOPY) -O binary $1 $2
+endef
+
+# Take the raw binary $1 and make it into an object file $2.
+# The symbol $3 points to the beginning of the file, and $3_len
+# gives its length.
+#
+# - compile an empty file to generate a suitable object file
+# - relink the object and insert the binary file
+# - edit symbol names to suit
+#
+# NOTE: exercise caution using this with absolute pathnames; it looks
+# like the MinGW tools insert an extra _ in the binary symbol name; e.g.
+# the path:
+#
+# /d/px4/firmware/Build/px4fmu_default.build/romfs.img
+#
+# is assigned symbols like:
+#
+# _binary_d__px4_firmware_Build_px4fmu_default_build_romfs_img_size
+#
+# when we would expect
+#
+# _binary__d_px4_firmware_Build_px4fmu_default_build_romfs_img_size
+#
+define BIN_SYM_PREFIX
+ _binary_$(subst /,_,$(subst .,_,$1))
+endef
+define BIN_TO_OBJ
+ @$(ECHO) "OBJ: $2"
+ @$(MKDIR) -p $(dir $2)
+ $(Q) $(ECHO) > $2.c
+ $(call COMPILE,$2.c,$2.c.o)
+ $(Q) $(LD) -r -o $2 $2.c.o -b binary $1
+ $(Q) $(OBJCOPY) $2 \
+ --redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \
+ --redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \
+ --strip-symbol $(call BIN_SYM_PREFIX,$1)_end
+ $(Q) $(REMOVE) $2.c $2.c.o
+endef
diff --git a/makefiles/upload.mk b/makefiles/upload.mk
new file mode 100644
index 000000000..5ef92728f
--- /dev/null
+++ b/makefiles/upload.mk
@@ -0,0 +1,44 @@
+#
+# Rules and tools for uploading firmware to various PX4 boards.
+#
+
+UPLOADER = $(PX4_BASE)/Tools/px_uploader.py
+
+SYSTYPE := $(shell uname -s)
+
+#
+# Serial port defaults.
+#
+# XXX The uploader should be smarter than this.
+#
+ifeq ($(SYSTYPE),Darwin)
+SERIAL_PORTS ?= "/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4,/dev/tty.usbmodem1,/dev/tty.usbmodem2,/dev/tty.usbmodem3,/dev/tty.usbmodem4"
+endif
+ifeq ($(SYSTYPE),Linux)
+SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0"
+endif
+ifeq ($(SERIAL_PORTS),)
+SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0"
+endif
+
+.PHONY: all upload-$(METHOD)-$(BOARD)
+all: upload-$(METHOD)-$(BOARD)
+
+upload-serial-px4fmu: $(BUNDLE) $(UPLOADER)
+ $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
+
+upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER)
+ $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
+
+#
+# JTAG firmware uploading with OpenOCD
+#
+JTAGCONFIG ?= interface/olimex-jtag-tiny.cfg
+
+upload-jtag-px4fmu: all
+ @$(ECHO) Attempting to flash PX4FMU board via JTAG
+ $(Q) $(OPENOCD) -f $(JTAGCONFIG) -f ../Bootloader/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4fmu_bl.elf" -c "reset run" -c shutdown
+
+upload-jtag-px4io: all
+ @$(ECHO) Attempting to flash PX4IO board via JTAG
+ $(Q) $(OPENOCD) -f $(JTAGCONFIG) -f ../Bootloader/stm32f1x.cfg -c init -c "reset halt" -c "flash write_image erase nuttx/nuttx" -c "flash write_image erase ../Bootloader/px4io_bl.elf" -c "reset run" -c shutdown
diff --git a/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c b/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c
index 2c9ae4cac..0fcd463e0 100644
--- a/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c
+++ b/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c
@@ -1512,7 +1512,6 @@ static inline void stm32_ep0out_receive(FAR struct stm32_ep_s *privep, int bcnt)
DEBUGASSERT(privep && privep->ep.priv);
priv = (FAR struct stm32_usbdev_s *)privep->ep.priv;
- DEBUGASSERT(priv->ep0state == EP0STATE_SETUP_OUT);
ullvdbg("EP0: bcnt=%d\n", bcnt);
usbtrace(TRACE_READ(EP0), bcnt);
diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h
index 8ad56a4c6..294b6c398 100755
--- a/nuttx/configs/px4fmu/include/board.h
+++ b/nuttx/configs/px4fmu/include/board.h
@@ -308,6 +308,10 @@
#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1
#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1
+#define GPIO_SPI2_MISO GPIO_SPI2_MISO_2
+#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_2
+#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2
+
#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2
#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1
#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2
@@ -321,11 +325,14 @@
#define PX4_SPIDEV_ACCEL 2
#define PX4_SPIDEV_MPU 3
+#define PX4_SPIDEV_ACCEL_MAG 2 // external for anti vibration test
+
/*
* Tone alarm output
*/
#define TONE_ALARM_TIMER 3 /* timer 3 */
#define TONE_ALARM_CHANNEL 3 /* channel 3 */
+#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
/************************************************************************************
diff --git a/nuttx/configs/px4fmu/include/nsh_romfsimg.h b/nuttx/configs/px4fmu/include/nsh_romfsimg.h
new file mode 100644
index 000000000..15e4e7a8d
--- /dev/null
+++ b/nuttx/configs/px4fmu/include/nsh_romfsimg.h
@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
+
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index 80cf6f312..0e18aa8ef 100644
--- a/nuttx/configs/px4fmu/nsh/appconfig
+++ b/nuttx/configs/px4fmu/nsh/appconfig
@@ -41,97 +41,6 @@ CONFIGURED_APPS += examples/nsh
CONFIGURED_APPS += nshlib
CONFIGURED_APPS += system/readline
-# System library - utility functions
-CONFIGURED_APPS += systemlib
-CONFIGURED_APPS += systemlib/mixer
-
-# Math library
-ifneq ($(CONFIG_APM),y)
-CONFIGURED_APPS += mathlib
-CONFIGURED_APPS += mathlib/CMSIS
-CONFIGURED_APPS += examples/math_demo
-endif
-
-# Control library
-ifneq ($(CONFIG_APM),y)
-CONFIGURED_APPS += controllib
-CONFIGURED_APPS += examples/control_demo
-CONFIGURED_APPS += examples/kalman_demo
-endif
-
-# System utility commands
-CONFIGURED_APPS += systemcmds/reboot
-CONFIGURED_APPS += systemcmds/perf
-CONFIGURED_APPS += systemcmds/top
-CONFIGURED_APPS += systemcmds/boardinfo
-CONFIGURED_APPS += systemcmds/mixer
-CONFIGURED_APPS += systemcmds/eeprom
-CONFIGURED_APPS += systemcmds/param
-CONFIGURED_APPS += systemcmds/pwm
-CONFIGURED_APPS += systemcmds/bl_update
-CONFIGURED_APPS += systemcmds/preflight_check
-CONFIGURED_APPS += systemcmds/delay_test
-
-# Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/hello_sky
-# CONFIGURED_APPS += examples/px4_simple_app
-
-# Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/deamon
-# CONFIGURED_APPS += examples/px4_deamon_app
-
-# Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/debug_values
-# CONFIGURED_APPS += examples/px4_mavlink_debug
-
-# Shared object broker; required by many parts of the system.
-CONFIGURED_APPS += uORB
-
-CONFIGURED_APPS += mavlink
-CONFIGURED_APPS += mavlink_onboard
-CONFIGURED_APPS += commander
-CONFIGURED_APPS += sdlog
-CONFIGURED_APPS += sensors
-
-ifneq ($(CONFIG_APM),y)
-CONFIGURED_APPS += ardrone_interface
-CONFIGURED_APPS += multirotor_att_control
-CONFIGURED_APPS += multirotor_pos_control
-#CONFIGURED_APPS += fixedwing_control
-CONFIGURED_APPS += fixedwing_att_control
-CONFIGURED_APPS += fixedwing_pos_control
-CONFIGURED_APPS += position_estimator
-CONFIGURED_APPS += attitude_estimator_ekf
-CONFIGURED_APPS += hott_telemetry
-endif
-
-# Hacking tools
-#CONFIGURED_APPS += system/i2c
-CONFIGURED_APPS += systemcmds/i2c
-
-# Communication and Drivers
-CONFIGURED_APPS += drivers/boards/px4fmu
-CONFIGURED_APPS += drivers/device
-CONFIGURED_APPS += drivers/ms5611
-CONFIGURED_APPS += drivers/hmc5883
-CONFIGURED_APPS += drivers/mpu6000
-CONFIGURED_APPS += drivers/bma180
-CONFIGURED_APPS += drivers/l3gd20
-CONFIGURED_APPS += drivers/px4io
-CONFIGURED_APPS += drivers/stm32
-CONFIGURED_APPS += drivers/led
-CONFIGURED_APPS += drivers/blinkm
-CONFIGURED_APPS += drivers/stm32/tone_alarm
-CONFIGURED_APPS += drivers/stm32/adc
-CONFIGURED_APPS += drivers/px4fmu
-CONFIGURED_APPS += drivers/hil
-CONFIGURED_APPS += drivers/gps
-CONFIGURED_APPS += drivers/mb12xx
-
-# Testing stuff
-CONFIGURED_APPS += px4/sensors_bringup
-CONFIGURED_APPS += px4/tests
-
ifeq ($(CONFIG_CAN),y)
#CONFIGURED_APPS += examples/can
endif
diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig
index 130886aac..02e224302 100755
--- a/nuttx/configs/px4fmu/nsh/defconfig
+++ b/nuttx/configs/px4fmu/nsh/defconfig
@@ -85,7 +85,7 @@ CONFIG_ARCH_FPU=y
CONFIG_ARCH_INTERRUPTSTACK=n
CONFIG_ARCH_STACKDUMP=y
CONFIG_ARCH_BOOTLOADER=n
-CONFIG_ARCH_LEDS=y
+CONFIG_ARCH_LEDS=n
CONFIG_ARCH_BUTTONS=n
CONFIG_ARCH_CALIBRATION=n
CONFIG_ARCH_DMA=y
@@ -647,10 +647,14 @@ CONFIG_DISABLE_POLL=n
# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing:
# 5.1234567
# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu)
+# CONFIG_LIBC_STRERR - allow printing of error text
+# CONFIG_LIBC_STRERR_SHORT - allow printing of short error text
#
CONFIG_NOPRINTF_FIELDWIDTH=n
CONFIG_LIBC_FLOATINGPOINT=y
CONFIG_HAVE_LONG_LONG=y
+CONFIG_LIBC_STRERROR=n
+CONFIG_LIBC_STRERROR_SHORT=n
#
# Allow for architecture optimized implementations
diff --git a/nuttx/configs/px4io/common/Make.defs b/nuttx/configs/px4io/common/Make.defs
index 4958f7092..7f782b5b2 100644
--- a/nuttx/configs/px4io/common/Make.defs
+++ b/nuttx/configs/px4io/common/Make.defs
@@ -112,7 +112,6 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
ARCHOPTIMIZATION += -g
-ARCHSCRIPT += -g
endif
ARCHCFLAGS = -std=gnu99
@@ -145,7 +144,7 @@ ARCHDEFINES =
ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10
# this seems to be the only way to add linker flags
-ARCHSCRIPT += --warn-common \
+EXTRA_LIBS += --warn-common \
--gc-sections
CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
diff --git a/nuttx/configs/px4io/include/board.h b/nuttx/configs/px4io/include/board.h
index d941985b4..fcbc93620 100755
--- a/nuttx/configs/px4io/include/board.h
+++ b/nuttx/configs/px4io/include/board.h
@@ -100,12 +100,19 @@
* Some of the USART pins are not available; override the GPIO
* definitions with an invalid pin configuration.
*/
+#undef GPIO_USART2_CTS
#define GPIO_USART2_CTS 0xffffffff
+#undef GPIO_USART2_RTS
#define GPIO_USART2_RTS 0xffffffff
+#undef GPIO_USART2_CK
#define GPIO_USART2_CK 0xffffffff
+#undef GPIO_USART3_TX
#define GPIO_USART3_TX 0xffffffff
+#undef GPIO_USART3_CK
#define GPIO_USART3_CK 0xffffffff
+#undef GPIO_USART3_CTS
#define GPIO_USART3_CTS 0xffffffff
+#undef GPIO_USART3_RTS
#define GPIO_USART3_RTS 0xffffffff
/*
@@ -156,6 +163,10 @@ extern "C" {
************************************************************************************/
EXTERN void stm32_boardinitialize(void);
+
+#if defined(__cplusplus)
+}
+#endif
#endif /* __ASSEMBLY__ */
#endif /* __ARCH_BOARD_BOARD_H */
diff --git a/nuttx/configs/px4io/io/appconfig b/nuttx/configs/px4io/io/appconfig
index 628607a51..48a41bcdb 100644
--- a/nuttx/configs/px4io/io/appconfig
+++ b/nuttx/configs/px4io/io/appconfig
@@ -30,11 +30,3 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
-
-CONFIGURED_APPS += drivers/boards/px4io
-CONFIGURED_APPS += drivers/stm32
-
-CONFIGURED_APPS += px4io
-
-# Mixer library from systemlib
-CONFIGURED_APPS += systemlib/mixer
diff --git a/nuttx/libc/stdio/lib_libdtoa.c b/nuttx/libc/stdio/lib_libdtoa.c
index 29f61fd76..395a55b61 100644
--- a/nuttx/libc/stdio/lib_libdtoa.c
+++ b/nuttx/libc/stdio/lib_libdtoa.c
@@ -43,6 +43,7 @@
/****************************************************************************
* Included Files
****************************************************************************/
+#include <math.h>
/****************************************************************************
* Pre-processor Definitions
@@ -104,6 +105,13 @@ static void zeroes(FAR struct lib_outstream_s *obj, int nzeroes)
* Private Functions
****************************************************************************/
+static void lib_dtoa_string(FAR struct lib_outstream_s *obj, const char *str)
+{
+ while (*str) {
+ obj->put(obj, *str++);
+ }
+}
+
/****************************************************************************
* Name: lib_dtoa
*
@@ -137,9 +145,23 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
int nchars; /* Number of characters to print */
int dsgn; /* Unused sign indicator */
int i;
+ bool done_decimal_point = false;
+
+ /* special handling for NaN and Infinity */
+ if (isnan(value)) {
+ lib_dtoa_string(obj, "NaN");
+ return;
+ }
+ if (isinf(value)) {
+ if (value < 0.0d) {
+ obj->put(obj, '-');
+ }
+ lib_dtoa_string(obj, "Infinity");
+ return;
+ }
/* Non-zero... positive or negative */
-
+
if (value < 0)
{
value = -value;
@@ -178,6 +200,7 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
if (prec > 0 || IS_ALTFORM(flags))
{
obj->put(obj, '.');
+ done_decimal_point = true;
/* Always print at least one digit to the right of the decimal point. */
@@ -203,6 +226,7 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
/* Print the decimal point */
obj->put(obj, '.');
+ done_decimal_point = true;
/* Print any leading zeros to the right of the decimal point */
@@ -249,6 +273,7 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
/* Print the decimal point */
obj->put(obj, '.');
+ done_decimal_point = true;
/* Always print at least one digit to the right of the decimal
* point.
@@ -285,8 +310,9 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
}
/* Finally, print any trailing zeroes */
-
- zeroes(obj, prec);
+ if (done_decimal_point) {
+ zeroes(obj, prec);
+ }
/* Is this memory supposed to be freed or not? */
diff --git a/nuttx/libc/stdio/lib_libvsprintf.c b/nuttx/libc/stdio/lib_libvsprintf.c
index 9a391610d..2cc7950f7 100644
--- a/nuttx/libc/stdio/lib_libvsprintf.c
+++ b/nuttx/libc/stdio/lib_libvsprintf.c
@@ -1215,7 +1215,7 @@ int lib_vsprintf(FAR struct lib_outstream_s *obj, FAR const char *src, va_list a
fmt = FMT_RJUST;
width = 0;
#ifdef CONFIG_LIBC_FLOATINGPOINT
- trunc = 0;
+ trunc = 6;
#endif
#endif
@@ -1245,6 +1245,11 @@ int lib_vsprintf(FAR struct lib_outstream_s *obj, FAR const char *src, va_list a
{
#ifndef CONFIG_NOPRINTF_FIELDWIDTH
fmt = FMT_RJUST0;
+#ifdef CONFIG_LIBC_FLOATINGPOINT
+ if (IS_HASDOT(flags)) {
+ trunc = 0;
+ }
+#endif
#endif
}
#if 0
diff --git a/platforms/empty.c b/platforms/empty.c
new file mode 100644
index 000000000..139531354
--- /dev/null
+++ b/platforms/empty.c
@@ -0,0 +1,3 @@
+/*
+ * This is an empty C source file, used when building default firmware configurations.
+ */
diff --git a/apps/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 264041e65..264041e65 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index f15c74a22..ecd31a073 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -482,10 +482,10 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
/* Failsafe logic - should never be necessary */
- motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
- motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
- motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
- motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
+ motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
+ motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
+ motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
+ motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
/* send motors via UART */
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/src/drivers/ardrone_interface/ardrone_motor_control.h
index 78b603b63..78b603b63 100644
--- a/apps/ardrone_interface/ardrone_motor_control.h
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.h
diff --git a/src/drivers/ardrone_interface/module.mk b/src/drivers/ardrone_interface/module.mk
new file mode 100644
index 000000000..058bd1397
--- /dev/null
+++ b/src/drivers/ardrone_interface/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# AR.Drone motor driver
+#
+
+MODULE_COMMAND = ardrone_interface
+SRCS = ardrone_interface.c \
+ ardrone_motor_control.c
diff --git a/apps/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 60d51f9af..7a64b72a4 100644
--- a/apps/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -92,6 +92,7 @@
#include <nuttx/config.h>
+#include <arch/board/board.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
@@ -842,7 +843,7 @@ int
blinkm_main(int argc, char *argv[])
{
- int i2cdevice = 3;
+ int i2cdevice = PX4_I2C_BUS_EXPANSION;
int blinkmadr = 9;
int x;
diff --git a/apps/drivers/device/Makefile b/src/drivers/blinkm/module.mk
index f7b1fff88..b48b90f3f 100644
--- a/apps/drivers/device/Makefile
+++ b/src/drivers/blinkm/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,7 +32,9 @@
############################################################################
#
-# Build the device driver framework.
+# BlinkM I2C LED driver
#
-include $(APPDIR)/mk/app.mk
+MODULE_COMMAND = blinkm
+
+SRCS = blinkm.cpp
diff --git a/apps/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index 4409a8a9c..4409a8a9c 100644
--- a/apps/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
diff --git a/apps/drivers/bma180/Makefile b/src/drivers/bma180/module.mk
index cc01b629e..4c60ee082 100644
--- a/apps/drivers/bma180/Makefile
+++ b/src/drivers/bma180/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,8 +35,6 @@
# Makefile to build the BMA180 driver.
#
-APPNAME = bma180
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+MODULE_COMMAND = bma180
-include $(APPDIR)/mk/app.mk
+SRCS = bma180.cpp
diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu/module.mk
new file mode 100644
index 000000000..66b776917
--- /dev/null
+++ b/src/drivers/boards/px4fmu/module.mk
@@ -0,0 +1,10 @@
+#
+# Board-specific startup code for the PX4FMU
+#
+
+SRCS = px4fmu_can.c \
+ px4fmu_init.c \
+ px4fmu_pwm_servo.c \
+ px4fmu_spi.c \
+ px4fmu_usb.c \
+ px4fmu_led.c
diff --git a/apps/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu/px4fmu_can.c
index 0d0b5fcd3..187689ff9 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_can.c
+++ b/src/drivers/boards/px4fmu/px4fmu_can.c
@@ -37,7 +37,6 @@
* Board-specific CAN functions.
*/
-
/************************************************************************************
* Included Files
************************************************************************************/
@@ -57,6 +56,8 @@
#include "stm32_can.h"
#include "px4fmu_internal.h"
+#ifdef CONFIG_CAN
+
/************************************************************************************
* Pre-processor Definitions
************************************************************************************/
@@ -139,3 +140,5 @@ int can_devinit(void)
return OK;
}
+
+#endif
diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c
index 9960c6bbd..69edc23ab 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu/px4fmu_init.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -91,6 +91,19 @@
# endif
#endif
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
/****************************************************************************
* Protected Functions
****************************************************************************/
@@ -114,7 +127,7 @@ __EXPORT void stm32_boardinitialize(void)
/* configure SPI interfaces */
stm32_spiinitialize();
- /* configure LEDs */
+ /* configure LEDs (empty call to NuttX' ledinit) */
up_ledinit();
}
@@ -127,6 +140,7 @@ __EXPORT void stm32_boardinitialize(void)
****************************************************************************/
static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi2;
static struct spi_dev_s *spi3;
#include <math.h>
@@ -147,6 +161,11 @@ __EXPORT int nsh_archinitialize(void)
{
int result;
+ /* configure always-on ADC pins */
+ stm32_configgpio(GPIO_ADC1_IN10);
+ stm32_configgpio(GPIO_ADC1_IN11);
+ /* IN12 and IN13 further below */
+
/* configure the high-resolution time/callout interface */
hrt_init();
@@ -172,11 +191,11 @@ __EXPORT int nsh_archinitialize(void)
(hrt_callout)stm32_serial_dma_poll,
NULL);
- // initial LED state
+ /* initial LED state */
drv_led_start();
- up_ledoff(LED_BLUE);
- up_ledoff(LED_AMBER);
- up_ledon(LED_BLUE);
+ led_off(LED_AMBER);
+ led_on(LED_BLUE);
+
/* Configure SPI-based devices */
@@ -188,7 +207,7 @@ __EXPORT int nsh_archinitialize(void)
return -ENODEV;
}
- // Default SPI1 to 1MHz and de-assert the known chip selects.
+ /* Default SPI1 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
@@ -199,6 +218,28 @@ __EXPORT int nsh_archinitialize(void)
message("[boot] Successfully initialized SPI port 1\r\n");
+ /*
+ * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
+ * Keep the SPI2 init optional and conditionally initialize the ADC pins
+ */
+ spi2 = up_spiinitialize(2);
+
+ if (!spi2) {
+ message("[boot] Enabling IN12/13 instead of SPI2\n");
+ /* no SPI2, use pins for ADC */
+ stm32_configgpio(GPIO_ADC1_IN12);
+ stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
+ } else {
+ /* Default SPI2 to 1MHz and de-assert the known chip selects. */
+ SPI_SETFREQUENCY(spi2, 10000000);
+ SPI_SETBITS(spi2, 8);
+ SPI_SETMODE(spi2, SPIDEV_MODE3);
+ SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false);
+ SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
+
+ message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
+ }
+
/* Get the SPI port for the microSD slot */
message("[boot] Initializing SPI port 3\n");
@@ -223,10 +264,5 @@ __EXPORT int nsh_archinitialize(void)
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
- stm32_configgpio(GPIO_ADC1_IN10);
- stm32_configgpio(GPIO_ADC1_IN11);
- stm32_configgpio(GPIO_ADC1_IN12);
- stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
-
return OK;
}
diff --git a/apps/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h
index 6550fdf3d..671a58f8f 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_internal.h
+++ b/src/drivers/boards/px4fmu/px4fmu_internal.h
@@ -58,9 +58,9 @@ __BEGIN_DECLS
****************************************************************************************************/
/* Configuration ************************************************************************************/
-#ifdef CONFIG_STM32_SPI2
-# error "SPI2 is not supported on this board"
-#endif
+//#ifdef CONFIG_STM32_SPI2
+//# error "SPI2 is not supported on this board"
+//#endif
#if defined(CONFIG_STM32_CAN1)
# warning "CAN1 is not supported on this board"
@@ -77,6 +77,7 @@ __BEGIN_DECLS
// XXX MPU6000 DRDY?
/* SPI chip selects */
+
#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
#define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
diff --git a/apps/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu/px4fmu_led.c
index fd1e159aa..34fd194c3 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu/px4fmu_led.c
@@ -39,19 +39,27 @@
#include <nuttx/config.h>
-#include <stdint.h>
#include <stdbool.h>
-#include <debug.h>
-#include <arch/board/board.h>
-
-#include "chip.h"
-#include "up_arch.h"
-#include "up_internal.h"
#include "stm32_internal.h"
#include "px4fmu_internal.h"
-__EXPORT void up_ledinit()
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+__EXPORT void led_init()
{
/* Configure LED1-2 GPIOs for output */
@@ -59,7 +67,7 @@ __EXPORT void up_ledinit()
stm32_configgpio(GPIO_LED2);
}
-__EXPORT void up_ledon(int led)
+__EXPORT void led_on(int led)
{
if (led == 0)
{
@@ -73,7 +81,7 @@ __EXPORT void up_ledon(int led)
}
}
-__EXPORT void up_ledoff(int led)
+__EXPORT void led_off(int led)
{
if (led == 0)
{
diff --git a/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
index cb8918306..cb8918306 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c
+++ b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
diff --git a/apps/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu/px4fmu_spi.c
index 7a02eaeb7..b5d00eac0 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu/px4fmu_spi.c
@@ -121,6 +121,24 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
return SPI_STATUS_PRESENT;
}
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+ /* SPI select is active low, so write !selected to select the device */
+
+ switch (devid) {
+ break;
+
+ default:
+ break;
+
+ }
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+ return SPI_STATUS_PRESENT;
+}
+
__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
{
diff --git a/apps/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu/px4fmu_usb.c
index b0b669fbe..b0b669fbe 100644
--- a/apps/drivers/boards/px4fmu/px4fmu_usb.c
+++ b/src/drivers/boards/px4fmu/px4fmu_usb.c
diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io/module.mk
new file mode 100644
index 000000000..2601a1c15
--- /dev/null
+++ b/src/drivers/boards/px4io/module.mk
@@ -0,0 +1,6 @@
+#
+# Board-specific startup code for the PX4IO
+#
+
+SRCS = px4io_init.c \
+ px4io_pwm_servo.c
diff --git a/apps/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io/px4io_init.c
index 14e8dc13a..d36353c6f 100644
--- a/apps/drivers/boards/px4io/px4io_init.c
+++ b/src/drivers/boards/px4io/px4io_init.c
@@ -86,9 +86,17 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_SERVO_PWR_EN);
stm32_configgpio(GPIO_RELAY1_EN);
stm32_configgpio(GPIO_RELAY2_EN);
+
+ /* turn off - all leds are active low */
+ stm32_gpiowrite(GPIO_LED1, true);
+ stm32_gpiowrite(GPIO_LED2, true);
+ stm32_gpiowrite(GPIO_LED3, true);
+
+ /* LED config */
stm32_configgpio(GPIO_LED1);
stm32_configgpio(GPIO_LED2);
stm32_configgpio(GPIO_LED3);
+
stm32_configgpio(GPIO_ACC_OC_DETECT);
stm32_configgpio(GPIO_SERVO_OC_DETECT);
stm32_configgpio(GPIO_BTN_SAFETY);
diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io/px4io_internal.h
index 44bb98513..eb2820bb7 100644
--- a/apps/drivers/boards/px4io/px4io_internal.h
+++ b/src/drivers/boards/px4io/px4io_internal.h
@@ -32,7 +32,9 @@
****************************************************************************/
/**
- * @file PX4IO hardware definitions.
+ * @file px4io_internal.h
+ *
+ * PX4IO hardware definitions.
*/
#pragma once
diff --git a/apps/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io/px4io_pwm_servo.c
index a2f73c429..a2f73c429 100644
--- a/apps/drivers/boards/px4io/px4io_pwm_servo.c
+++ b/src/drivers/boards/px4io/px4io_pwm_servo.c
diff --git a/apps/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index 422321850..422321850 100644
--- a/apps/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
diff --git a/apps/drivers/device/device.cpp b/src/drivers/device/device.cpp
index 04a5222c3..04a5222c3 100644
--- a/apps/drivers/device/device.cpp
+++ b/src/drivers/device/device.cpp
diff --git a/apps/drivers/device/device.h b/src/drivers/device/device.h
index 7d375aab9..7d375aab9 100644
--- a/apps/drivers/device/device.h
+++ b/src/drivers/device/device.h
diff --git a/apps/drivers/device/i2c.cpp b/src/drivers/device/i2c.cpp
index a416801eb..a416801eb 100644
--- a/apps/drivers/device/i2c.cpp
+++ b/src/drivers/device/i2c.cpp
diff --git a/apps/drivers/device/i2c.h b/src/drivers/device/i2c.h
index 66c34dd7c..b4a9cdd53 100644
--- a/apps/drivers/device/i2c.h
+++ b/src/drivers/device/i2c.h
@@ -53,6 +53,15 @@ namespace device __EXPORT
class __EXPORT I2C : public CDev
{
+public:
+
+ /**
+ * Get the address
+ */
+ uint16_t get_address() {
+ return _address;
+ }
+
protected:
/**
* The number of times a read or write operation will be retried on
@@ -61,6 +70,11 @@ protected:
unsigned _retries;
/**
+ * The I2C bus number the device is attached to.
+ */
+ int _bus;
+
+ /**
* @ Constructor
*
* @param name Driver name
@@ -123,7 +137,6 @@ protected:
}
private:
- int _bus;
uint16_t _address;
uint32_t _frequency;
struct i2c_dev_s *_dev;
diff --git a/apps/drivers/hmc5883/Makefile b/src/drivers/device/module.mk
index 4d7cb4e7b..8c716d9cd 100644
--- a/apps/drivers/hmc5883/Makefile
+++ b/src/drivers/device/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,11 +32,11 @@
############################################################################
#
-# HMC5883 driver
+# Build the device driver framework.
#
-APPNAME = hmc5883
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 4096
-
-include $(APPDIR)/mk/app.mk
+SRCS = cdev.cpp \
+ device.cpp \
+ i2c.cpp \
+ pio.cpp \
+ spi.cpp
diff --git a/apps/drivers/device/pio.cpp b/src/drivers/device/pio.cpp
index f3a805a5e..f3a805a5e 100644
--- a/apps/drivers/device/pio.cpp
+++ b/src/drivers/device/pio.cpp
diff --git a/apps/drivers/device/spi.cpp b/src/drivers/device/spi.cpp
index 8fffd60cb..8fffd60cb 100644
--- a/apps/drivers/device/spi.cpp
+++ b/src/drivers/device/spi.cpp
diff --git a/apps/drivers/device/spi.h b/src/drivers/device/spi.h
index d2d01efb3..d2d01efb3 100644
--- a/apps/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
diff --git a/apps/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 794de584b..794de584b 100644
--- a/apps/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
diff --git a/apps/drivers/drv_adc.h b/src/drivers/drv_adc.h
index 8ec6d1233..8ec6d1233 100644
--- a/apps/drivers/drv_adc.h
+++ b/src/drivers/drv_adc.h
diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h
new file mode 100644
index 000000000..bffc35c62
--- /dev/null
+++ b/src/drivers/drv_airspeed.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Airspeed driver interface.
+ * @author Simon Wilks
+ */
+
+#ifndef _DRV_AIRSPEED_H
+#define _DRV_AIRSPEED_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define AIRSPEED_DEVICE_PATH "/dev/airspeed"
+
+/*
+ * ioctl() definitions
+ *
+ * Airspeed drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
+
+#define _AIRSPEEDIOCBASE (0x7700)
+#define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n))
+
+
+#endif /* _DRV_AIRSPEED_H */
diff --git a/apps/drivers/drv_baro.h b/src/drivers/drv_baro.h
index 176f798c0..176f798c0 100644
--- a/apps/drivers/drv_baro.h
+++ b/src/drivers/drv_baro.h
diff --git a/apps/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h
index 9c278f6c5..9c278f6c5 100644
--- a/apps/drivers/drv_blinkm.h
+++ b/src/drivers/drv_blinkm.h
diff --git a/apps/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 92d184326..92d184326 100644
--- a/apps/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
diff --git a/apps/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 1dda8ef0b..1dda8ef0b 100644
--- a/apps/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
diff --git a/apps/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 1d0fef6fc..1d0fef6fc 100644
--- a/apps/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
diff --git a/apps/drivers/drv_hrt.h b/src/drivers/drv_hrt.h
index 8a99eeca7..8a99eeca7 100644
--- a/apps/drivers/drv_hrt.h
+++ b/src/drivers/drv_hrt.h
diff --git a/apps/drivers/drv_led.h b/src/drivers/drv_led.h
index 21044f620..21044f620 100644
--- a/apps/drivers/drv_led.h
+++ b/src/drivers/drv_led.h
diff --git a/apps/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 9aab995a1..9aab995a1 100644
--- a/apps/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
diff --git a/apps/drivers/drv_mixer.h b/src/drivers/drv_mixer.h
index 9f43015d9..9f43015d9 100644
--- a/apps/drivers/drv_mixer.h
+++ b/src/drivers/drv_mixer.h
diff --git a/apps/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h
index 8495780d5..713618545 100644
--- a/apps/drivers/drv_orb_dev.h
+++ b/src/drivers/drv_orb_dev.h
@@ -43,7 +43,7 @@
#include <stdint.h>
/* XXX for ORB_DECLARE used in many drivers */
-#include "../uORB/uORB.h"
+#include "../modules/uORB/uORB.h"
/*
* ioctl() definitions
diff --git a/apps/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 07831f20c..56af71059 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm);
/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
+/** set the 'ARM ok' bit, which activates the safety switch */
+#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5)
+
+/** clear the 'ARM ok' bit, which deactivates the safety switch */
+#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6)
+
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
diff --git a/apps/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index 03a82ec5d..03a82ec5d 100644
--- a/apps/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
diff --git a/apps/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 4decc324e..4decc324e 100644
--- a/apps/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
diff --git a/apps/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 3a89cab9d..3a89cab9d 100644
--- a/apps/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
diff --git a/apps/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index 0c6afc64c..0c6afc64c 100644
--- a/apps/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
new file mode 100644
index 000000000..e50395e47
--- /dev/null
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -0,0 +1,832 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ets_airspeed.cpp
+ * @author Simon Wilks
+ *
+ * Driver for the Eagle Tree Airspeed V3 connected via I2C.
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/board.h>
+
+#include <systemlib/airspeed.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/subsystem_info.h>
+
+/* Default I2C bus */
+#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
+
+/* I2C bus address */
+#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */
+
+/* Register address */
+#define READ_CMD 0x07 /* Read the data */
+
+/**
+ * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h.
+ */
+#define MIN_ACCURATE_DIFF_PRES_PA 12
+
+/* Measurement rate is 100Hz */
+#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
+
+/* Oddly, ERROR is not defined for C++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class ETSAirspeed : public device::I2C
+{
+public:
+ ETSAirspeed(int bus, int address = I2C_ADDRESS);
+ virtual ~ETSAirspeed();
+
+ virtual int init();
+
+ virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
+ virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+ /**
+ * Diagnostics - print some basic information about the driver.
+ */
+ void print_info();
+
+protected:
+ virtual int probe();
+
+private:
+ work_s _work;
+ unsigned _num_reports;
+ volatile unsigned _next_report;
+ volatile unsigned _oldest_report;
+ differential_pressure_s *_reports;
+ bool _sensor_ok;
+ int _measure_ticks;
+ bool _collect_phase;
+ int _diff_pres_offset;
+
+ orb_advert_t _airspeed_pub;
+
+ perf_counter_t _sample_perf;
+ perf_counter_t _comms_errors;
+ perf_counter_t _buffer_overflows;
+
+
+ /**
+ * Test whether the device supported by the driver is present at a
+ * specific address.
+ *
+ * @param address The I2C bus address to probe.
+ * @return True if the device is present.
+ */
+ int probe_address(uint8_t address);
+
+ /**
+ * Initialise the automatic measurement state machine and start it.
+ *
+ * @note This function is called at open and error time. It might make sense
+ * to make it more aggressive about resetting the bus in case of errors.
+ */
+ void start();
+
+ /**
+ * Stop the automatic measurement state machine.
+ */
+ void stop();
+
+ /**
+ * Perform a poll cycle; collect from the previous measurement
+ * and start a new one.
+ */
+ void cycle();
+ int measure();
+ int collect();
+
+ /**
+ * Static trampoline from the workq context; because we don't have a
+ * generic workq wrapper yet.
+ *
+ * @param arg Instance pointer for the driver that is polling.
+ */
+ static void cycle_trampoline(void *arg);
+
+
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
+
+ETSAirspeed::ETSAirspeed(int bus, int address) :
+ I2C("ETSAirspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
+ _num_reports(0),
+ _next_report(0),
+ _oldest_report(0),
+ _reports(nullptr),
+ _sensor_ok(false),
+ _measure_ticks(0),
+ _collect_phase(false),
+ _diff_pres_offset(0),
+ _airspeed_pub(-1),
+ _sample_perf(perf_alloc(PC_ELAPSED, "ets_airspeed_read")),
+ _comms_errors(perf_alloc(PC_COUNT, "ets_airspeed_comms_errors")),
+ _buffer_overflows(perf_alloc(PC_COUNT, "ets_airspeed_buffer_overflows"))
+{
+ // enable debug() calls
+ _debug_enabled = true;
+
+ // work_cancel in the dtor will explode if we don't do this...
+ memset(&_work, 0, sizeof(_work));
+}
+
+ETSAirspeed::~ETSAirspeed()
+{
+ /* make sure we are truly inactive */
+ stop();
+
+ /* free any existing reports */
+ if (_reports != nullptr)
+ delete[] _reports;
+}
+
+int
+ETSAirspeed::init()
+{
+ int ret = ERROR;
+
+ /* do I2C init (and probe) first */
+ if (I2C::init() != OK)
+ goto out;
+
+ /* allocate basic report buffers */
+ _num_reports = 2;
+ _reports = new struct differential_pressure_s[_num_reports];
+ for (unsigned i = 0; i < _num_reports; i++)
+ _reports[i].max_differential_pressure_pa = 0;
+
+ if (_reports == nullptr)
+ goto out;
+
+ _oldest_report = _next_report = 0;
+
+ /* get a publish handle on the airspeed topic */
+ memset(&_reports[0], 0, sizeof(_reports[0]));
+ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
+
+ if (_airspeed_pub < 0)
+ debug("failed to create airspeed sensor object. Did you start uOrb?");
+
+ ret = OK;
+ /* sensor is ok, but we don't really know if it is within range */
+ _sensor_ok = true;
+out:
+ return ret;
+}
+
+int
+ETSAirspeed::probe()
+{
+ return measure();
+}
+
+int
+ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ switch (cmd) {
+
+ case SENSORIOCSPOLLRATE: {
+ switch (arg) {
+
+ /* switching to manual polling */
+ case SENSOR_POLLRATE_MANUAL:
+ stop();
+ _measure_ticks = 0;
+ return OK;
+
+ /* external signalling (DRDY) not supported */
+ case SENSOR_POLLRATE_EXTERNAL:
+
+ /* zero would be bad */
+ case 0:
+ return -EINVAL;
+
+ /* set default/max polling rate */
+ case SENSOR_POLLRATE_MAX:
+ case SENSOR_POLLRATE_DEFAULT: {
+ /* do we need to start internal polling? */
+ bool want_start = (_measure_ticks == 0);
+
+ /* set interval for next measurement to minimum legal value */
+ _measure_ticks = USEC2TICK(CONVERSION_INTERVAL);
+
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
+
+ return OK;
+ }
+
+ /* adjust to a legal polling interval in Hz */
+ default: {
+ /* do we need to start internal polling? */
+ bool want_start = (_measure_ticks == 0);
+
+ /* convert hz to tick interval via microseconds */
+ unsigned ticks = USEC2TICK(1000000 / arg);
+
+ /* check against maximum rate */
+ if (ticks < USEC2TICK(CONVERSION_INTERVAL))
+ return -EINVAL;
+
+ /* update interval for next measurement */
+ _measure_ticks = ticks;
+
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
+
+ return OK;
+ }
+ }
+ }
+
+ case SENSORIOCGPOLLRATE:
+ if (_measure_ticks == 0)
+ return SENSOR_POLLRATE_MANUAL;
+
+ return (1000 / _measure_ticks);
+
+ case SENSORIOCSQUEUEDEPTH: {
+ /* add one to account for the sentinel in the ring */
+ arg++;
+
+ /* lower bound is mandatory, upper bound is a sanity check */
+ if ((arg < 2) || (arg > 100))
+ return -EINVAL;
+
+ /* allocate new buffer */
+ struct differential_pressure_s *buf = new struct differential_pressure_s[arg];
+
+ if (nullptr == buf)
+ return -ENOMEM;
+
+ /* reset the measurement state machine with the new buffer, free the old */
+ stop();
+ delete[] _reports;
+ _num_reports = arg;
+ _reports = buf;
+ start();
+
+ return OK;
+ }
+
+ case SENSORIOCGQUEUEDEPTH:
+ return _num_reports - 1;
+
+ case SENSORIOCRESET:
+ /* XXX implement this */
+ return -EINVAL;
+
+ default:
+ /* give it to the superclass */
+ return I2C::ioctl(filp, cmd, arg);
+ }
+}
+
+ssize_t
+ETSAirspeed::read(struct file *filp, char *buffer, size_t buflen)
+{
+ unsigned count = buflen / sizeof(struct differential_pressure_s);
+ int ret = 0;
+
+ /* buffer must be large enough */
+ if (count < 1)
+ return -ENOSPC;
+
+ /* if automatic measurement is enabled */
+ if (_measure_ticks > 0) {
+
+ /*
+ * While there is space in the caller's buffer, and reports, copy them.
+ * Note that we may be pre-empted by the workq thread while we are doing this;
+ * we are careful to avoid racing with them.
+ */
+ while (count--) {
+ if (_oldest_report != _next_report) {
+ memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+ ret += sizeof(_reports[0]);
+ INCREMENT(_oldest_report, _num_reports);
+ }
+ }
+
+ /* if there was no data, warn the caller */
+ return ret ? ret : -EAGAIN;
+ }
+
+ /* manual measurement - run one conversion */
+ /* XXX really it'd be nice to lock against other readers here */
+ do {
+ _oldest_report = _next_report = 0;
+
+ /* trigger a measurement */
+ if (OK != measure()) {
+ ret = -EIO;
+ break;
+ }
+
+ /* wait for it to complete */
+ usleep(CONVERSION_INTERVAL);
+
+ /* run the collection phase */
+ if (OK != collect()) {
+ ret = -EIO;
+ break;
+ }
+
+ /* state machine will have generated a report, copy it out */
+ memcpy(buffer, _reports, sizeof(*_reports));
+ ret = sizeof(*_reports);
+
+ } while (0);
+
+ return ret;
+}
+
+int
+ETSAirspeed::measure()
+{
+ int ret;
+
+ /*
+ * Send the command to begin a measurement.
+ */
+ uint8_t cmd = READ_CMD;
+ ret = transfer(&cmd, 1, nullptr, 0);
+
+ if (OK != ret)
+ {
+ perf_count(_comms_errors);
+ log("i2c::transfer returned %d", ret);
+ return ret;
+ }
+ ret = OK;
+
+ return ret;
+}
+
+int
+ETSAirspeed::collect()
+{
+ int ret = -EIO;
+
+ /* read from the sensor */
+ uint8_t val[2] = {0, 0};
+
+ perf_begin(_sample_perf);
+
+ ret = transfer(nullptr, 0, &val[0], 2);
+
+ if (ret < 0) {
+ log("error reading from sensor: %d", ret);
+ return ret;
+ }
+
+ uint16_t diff_pres_pa = val[1] << 8 | val[0];
+
+ param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
+
+ if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
+ diff_pres_pa = 0;
+ } else {
+ diff_pres_pa -= _diff_pres_offset;
+ }
+
+ // XXX we may want to smooth out the readings to remove noise.
+
+ _reports[_next_report].timestamp = hrt_absolute_time();
+ _reports[_next_report].differential_pressure_pa = diff_pres_pa;
+
+ // Track maximum differential pressure measured (so we can work out top speed).
+ if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
+ _reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
+ }
+
+ /* announce the airspeed if needed, just publish else */
+ orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
+
+ /* post a report to the ring - note, not locked */
+ INCREMENT(_next_report, _num_reports);
+
+ /* if we are running up against the oldest report, toss it */
+ if (_next_report == _oldest_report) {
+ perf_count(_buffer_overflows);
+ INCREMENT(_oldest_report, _num_reports);
+ }
+
+ /* notify anyone waiting for data */
+ poll_notify(POLLIN);
+
+ ret = OK;
+
+ perf_end(_sample_perf);
+
+ return ret;
+}
+
+void
+ETSAirspeed::start()
+{
+ /* reset the report ring and state machine */
+ _collect_phase = false;
+ _oldest_report = _next_report = 0;
+
+ /* schedule a cycle to start things */
+ work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1);
+
+ /* notify about state change */
+ struct subsystem_info_s info = {
+ true,
+ true,
+ true,
+ SUBSYSTEM_TYPE_DIFFPRESSURE};
+ static orb_advert_t pub = -1;
+
+ if (pub > 0) {
+ orb_publish(ORB_ID(subsystem_info), pub, &info);
+ } else {
+ pub = orb_advertise(ORB_ID(subsystem_info), &info);
+ }
+}
+
+void
+ETSAirspeed::stop()
+{
+ work_cancel(HPWORK, &_work);
+}
+
+void
+ETSAirspeed::cycle_trampoline(void *arg)
+{
+ ETSAirspeed *dev = (ETSAirspeed *)arg;
+
+ dev->cycle();
+}
+
+void
+ETSAirspeed::cycle()
+{
+ /* collection phase? */
+ if (_collect_phase) {
+
+ /* perform collection */
+ if (OK != collect()) {
+ log("collection error");
+ /* restart the measurement state machine */
+ start();
+ return;
+ }
+
+ /* next phase is measurement */
+ _collect_phase = false;
+
+ /*
+ * Is there a collect->measure gap?
+ */
+ if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
+
+ /* schedule a fresh cycle call when we are ready to measure again */
+ work_queue(HPWORK,
+ &_work,
+ (worker_t)&ETSAirspeed::cycle_trampoline,
+ this,
+ _measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
+
+ return;
+ }
+ }
+
+ /* measurement phase */
+ if (OK != measure())
+ log("measure error");
+
+ /* next phase is collection */
+ _collect_phase = true;
+
+ /* schedule a fresh cycle call when the measurement is done */
+ work_queue(HPWORK,
+ &_work,
+ (worker_t)&ETSAirspeed::cycle_trampoline,
+ this,
+ USEC2TICK(CONVERSION_INTERVAL));
+}
+
+void
+ETSAirspeed::print_info()
+{
+ perf_print_counter(_sample_perf);
+ perf_print_counter(_comms_errors);
+ perf_print_counter(_buffer_overflows);
+ printf("poll interval: %u ticks\n", _measure_ticks);
+ printf("report queue: %u (%u/%u @ %p)\n",
+ _num_reports, _oldest_report, _next_report, _reports);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace ets_airspeed
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+ETSAirspeed *g_dev;
+
+void start(int i2c_bus);
+void stop();
+void test();
+void reset();
+void info();
+
+/**
+ * Start the driver.
+ */
+void
+start(int i2c_bus)
+{
+ int fd;
+
+ if (g_dev != nullptr)
+ errx(1, "already started");
+
+ /* create the driver */
+ g_dev = new ETSAirspeed(i2c_bus);
+
+ if (g_dev == nullptr)
+ goto fail;
+
+ if (OK != g_dev->init())
+ goto fail;
+
+ /* set the poll rate to default, starts automatic data collection */
+ fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+
+ if (fd < 0)
+ goto fail;
+
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+ goto fail;
+
+ exit(0);
+
+fail:
+
+ if (g_dev != nullptr)
+ {
+ delete g_dev;
+ g_dev = nullptr;
+ }
+
+ errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void
+stop()
+{
+ if (g_dev != nullptr)
+ {
+ delete g_dev;
+ g_dev = nullptr;
+ }
+ else
+ {
+ errx(1, "driver not running");
+ }
+ exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+ struct differential_pressure_s report;
+ ssize_t sz;
+ int ret;
+
+ int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+
+ if (fd < 0)
+ err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH);
+
+ /* do a simple demand read */
+ sz = read(fd, &report, sizeof(report));
+
+ if (sz != sizeof(report))
+ err(1, "immediate read failed");
+
+ warnx("single read");
+ warnx("diff pressure: %d pa", report.differential_pressure_pa);
+
+ /* start the sensor polling at 2Hz */
+ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+ errx(1, "failed to set 2Hz poll rate");
+
+ /* read the sensor 5x and report each value */
+ for (unsigned i = 0; i < 5; i++) {
+ struct pollfd fds;
+
+ /* wait for data to be ready */
+ fds.fd = fd;
+ fds.events = POLLIN;
+ ret = poll(&fds, 1, 2000);
+
+ if (ret != 1)
+ errx(1, "timed out waiting for sensor data");
+
+ /* now go get it */
+ sz = read(fd, &report, sizeof(report));
+
+ if (sz != sizeof(report))
+ err(1, "periodic read failed");
+
+ warnx("periodic read %u", i);
+ warnx("diff pressure: %d pa", report.differential_pressure_pa);
+ }
+
+ errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+ int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+
+ if (fd < 0)
+ err(1, "failed ");
+
+ if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+ err(1, "driver reset failed");
+
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+ err(1, "driver poll restart failed");
+
+ exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+ if (g_dev == nullptr)
+ errx(1, "driver not running");
+
+ printf("state @ %p\n", g_dev);
+ g_dev->print_info();
+
+ exit(0);
+}
+
+} // namespace
+
+
+static void
+ets_airspeed_usage()
+{
+ fprintf(stderr, "usage: ets_airspeed [options] command\n");
+ fprintf(stderr, "options:\n");
+ fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT);
+ fprintf(stderr, "command:\n");
+ fprintf(stderr, "\tstart|stop|reset|test|info\n");
+}
+
+int
+ets_airspeed_main(int argc, char *argv[])
+{
+ int i2c_bus = PX4_I2C_BUS_DEFAULT;
+
+ int i;
+ for (i = 1; i < argc; i++) {
+ if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
+ if (argc > i + 1) {
+ i2c_bus = atoi(argv[i + 1]);
+ }
+ }
+ }
+
+ /*
+ * Start/load the driver.
+ */
+ if (!strcmp(argv[1], "start"))
+ ets_airspeed::start(i2c_bus);
+
+ /*
+ * Stop the driver
+ */
+ if (!strcmp(argv[1], "stop"))
+ ets_airspeed::stop();
+
+ /*
+ * Test the driver/device.
+ */
+ if (!strcmp(argv[1], "test"))
+ ets_airspeed::test();
+
+ /*
+ * Reset the driver.
+ */
+ if (!strcmp(argv[1], "reset"))
+ ets_airspeed::reset();
+
+ /*
+ * Print driver information.
+ */
+ if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+ ets_airspeed::info();
+
+ ets_airspeed_usage();
+ exit(0);
+}
diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk
new file mode 100644
index 000000000..cb5d3b1ed
--- /dev/null
+++ b/src/drivers/ets_airspeed/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+# Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the Eagle Tree Airspeed V3 driver.
+#
+
+MODULE_COMMAND = ets_airspeed
+MODULE_STACKSIZE = 1024
+
+SRCS = ets_airspeed.cpp
diff --git a/apps/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index e35bdb944..38835418b 100644
--- a/apps/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -285,6 +285,10 @@ GPS::task_main()
unlock();
if (_Helper->configure(_baudrate) == 0) {
unlock();
+
+ // GPS is obviously detected successfully, reset statistics
+ _Helper->reset_update_rates();
+
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
@@ -301,6 +305,8 @@ GPS::task_main()
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
+ _Helper->store_update_rates();
+ _Helper->reset_update_rates();
}
if (!_healthy) {
@@ -372,7 +378,10 @@ GPS::print_info()
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
- warnx("update rate: %6.2f Hz", (double)_rate);
+ warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
+ warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
+ warnx("rate publication:\t%6.2f Hz", (double)_rate);
+
}
usleep(100000);
diff --git a/apps/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index 9c1fad569..ba86d370a 100644
--- a/apps/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
@@ -36,9 +36,39 @@
#include <termios.h>
#include <errno.h>
#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
#include "gps_helper.h"
-/* @file gps_helper.cpp */
+/**
+ * @file gps_helper.cpp
+ */
+
+float
+GPS_Helper::get_position_update_rate()
+{
+ return _rate_lat_lon;
+}
+
+float
+GPS_Helper::get_velocity_update_rate()
+{
+ return _rate_vel;
+}
+
+float
+GPS_Helper::reset_update_rates()
+{
+ _rate_count_vel = 0;
+ _rate_count_lat_lon = 0;
+ _interval_rate_start = hrt_absolute_time();
+}
+
+float
+GPS_Helper::store_update_rates()
+{
+ _rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
+ _rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
+}
int
GPS_Helper::set_baudrate(const int &fd, unsigned baud)
diff --git a/apps/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index f3d3bc40b..defc1a074 100644
--- a/apps/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
@@ -33,7 +33,9 @@
*
****************************************************************************/
-/* @file gps_helper.h */
+/**
+ * @file gps_helper.h
+ */
#ifndef GPS_HELPER_H
#define GPS_HELPER_H
@@ -44,9 +46,22 @@
class GPS_Helper
{
public:
- virtual int configure(unsigned &baud) = 0;
+ virtual int configure(unsigned &baud) = 0;
virtual int receive(unsigned timeout) = 0;
- int set_baudrate(const int &fd, unsigned baud);
+ int set_baudrate(const int &fd, unsigned baud);
+ float get_position_update_rate();
+ float get_velocity_update_rate();
+ float reset_update_rates();
+ float store_update_rates();
+
+protected:
+ uint8_t _rate_count_lat_lon;
+ uint8_t _rate_count_vel;
+
+ float _rate_lat_lon;
+ float _rate_vel;
+
+ uint64_t _interval_rate_start;
};
#endif /* GPS_HELPER_H */
diff --git a/apps/drivers/gps/Makefile b/src/drivers/gps/module.mk
index 3859a88a5..097db2abf 100644
--- a/apps/drivers/gps/Makefile
+++ b/src/drivers/gps/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,8 +35,9 @@
# GPS driver
#
-APPNAME = gps
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+MODULE_COMMAND = gps
-include $(APPDIR)/mk/app.mk
+SRCS = gps.cpp \
+ gps_helper.cpp \
+ mtk.cpp \
+ ubx.cpp
diff --git a/apps/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 4762bd503..62941d74b 100644
--- a/apps/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -263,6 +263,10 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
+ // Position and velocity update always at the same time
+ _rate_count_vel++;
+ _rate_count_lat_lon++;
+
return;
}
diff --git a/apps/drivers/gps/mtk.h b/src/drivers/gps/mtk.h
index d4e390b01..b5cfbf0a6 100644
--- a/apps/drivers/gps/mtk.h
+++ b/src/drivers/gps/mtk.h
@@ -87,14 +87,14 @@ class MTK : public GPS_Helper
public:
MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
~MTK();
- int receive(unsigned timeout);
- int configure(unsigned &baudrate);
+ int receive(unsigned timeout);
+ int configure(unsigned &baudrate);
private:
/**
* Parse the binary MTK packet
*/
- int parse_char(uint8_t b, gps_mtk_packet_t &packet);
+ int parse_char(uint8_t b, gps_mtk_packet_t &packet);
/**
* Handle the package once it has arrived
diff --git a/apps/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 74cbc5aaf..b3093b0f6 100644
--- a/apps/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -33,7 +33,14 @@
*
****************************************************************************/
-/* @file U-Blox protocol implementation */
+/**
+ * @file ubx.cpp
+ *
+ * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
+ * including Prototol Specification.
+ *
+ * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
+ */
#include <unistd.h>
#include <stdio.h>
@@ -53,7 +60,8 @@
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
-_waiting_for_ack(false)
+_waiting_for_ack(false),
+_disable_cmd_counter(0)
{
decode_init();
}
@@ -113,13 +121,15 @@ UBX::configure(unsigned &baudrate)
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
+
+ /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
+ receive(UBX_CONFIG_TIMEOUT);
+
if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
}
- /* no ack is ecpected here, keep going configuring */
-
/* send a CFT-RATE message to define update rate */
type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
@@ -130,12 +140,12 @@ UBX::configure(unsigned &baudrate)
cfg_rate_packet.clsID = UBX_CLASS_CFG;
cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.length = UBX_CFG_RATE_LENGTH;
- cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE;
+ cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
- if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+ if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
@@ -155,74 +165,41 @@ UBX::configure(unsigned &baudrate)
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
- if (receive(UBX_CONFIG_TIMEOUT) < 0) {
- /* try next baudrate */
- continue;
- }
-
- type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
- memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
-
- _clsID_needed = UBX_CLASS_CFG;
- _msgID_needed = UBX_MESSAGE_CFG_MSG;
-
- cfg_msg_packet.clsID = UBX_CLASS_CFG;
- cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG;
- cfg_msg_packet.length = UBX_CFG_MSG_LENGTH;
- /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
- cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
-
- cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
- cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
-
- send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
- if (receive(UBX_CONFIG_TIMEOUT) < 0) {
- /* try next baudrate */
- continue;
- }
-
- cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
- cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
-
- send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
- if (receive(UBX_CONFIG_TIMEOUT) < 0) {
- /* try next baudrate */
- continue;
- }
-
- cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
- cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
- /* For satelites info 1Hz is enough */
- cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
-
- send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
- if (receive(UBX_CONFIG_TIMEOUT) < 0) {
- /* try next baudrate */
- continue;
- }
-
- cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
- cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
-
- send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
- if (receive(UBX_CONFIG_TIMEOUT) < 0) {
- /* try next baudrate */
- continue;
- }
-
- cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
- cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
-
- send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
- if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+ if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
-// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
-// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
-// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
+ configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
+ UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
+ /* insist of receiving the ACK for this packet */
+ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+ // continue;
+ configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
+ 1);
+ // /* insist of receiving the ACK for this packet */
+ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+ // continue;
+ configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
+ 1);
+ // /* insist of receiving the ACK for this packet */
+ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+ // continue;
+ configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
+ 1);
+ // /* insist of receiving the ACK for this packet */
+ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+ // continue;
+ // configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP,
+ // 0);
+ // /* insist of receiving the ACK for this packet */
+ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+ // continue;
+ configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
+ 0);
+ // /* insist of receiving the ACK for this packet */
+ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+ // continue;
_waiting_for_ack = false;
return 0;
@@ -231,6 +208,15 @@ UBX::configure(unsigned &baudrate)
}
int
+UBX::wait_for_ack(unsigned timeout)
+{
+ _waiting_for_ack = true;
+ int ret = receive(timeout);
+ _waiting_for_ack = false;
+ return ret;
+}
+
+int
UBX::receive(unsigned timeout)
{
/* poll descriptor */
@@ -489,6 +475,8 @@ UBX::handle_message()
_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
+ _rate_count_lat_lon++;
+
/* Add timestamp to finish the report */
_gps_position->timestamp_position = hrt_absolute_time();
/* only return 1 when new position is available */
@@ -631,18 +619,21 @@ UBX::handle_message()
}
case NAV_VELNED: {
-// printf("GOT NAV_VELNED MESSAGE\n");
if (!_waiting_for_ack) {
+ /* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */
gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
_gps_position->vel_m_s = (float)packet->speed * 1e-2f;
- _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f;
- _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f;
- _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f;
+ _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */
+ _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */
+ _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */
_gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
+ _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
_gps_position->vel_ned_valid = true;
_gps_position->timestamp_velocity = hrt_absolute_time();
+
+ _rate_count_vel++;
}
break;
@@ -683,6 +674,12 @@ UBX::handle_message()
default: //we don't know the message
warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
+ if (_disable_cmd_counter++ == 0) {
+ // Don't attempt for every message to disable, some might not be disabled */
+ warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+ configure_message_rate(_message_class, _message_id, 0);
+ }
+ return ret;
ret = -1;
break;
}
@@ -727,6 +724,25 @@ UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
}
void
+UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
+{
+ for (unsigned i = 0; i < length; i++) {
+ ck_a = ck_a + message[i];
+ ck_b = ck_b + ck_a;
+ }
+}
+
+void
+UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
+{
+ struct ubx_cfg_msg_rate msg;
+ msg.msg_class = msg_class;
+ msg.msg_id = msg_id;
+ msg.rate = rate;
+ send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
+}
+
+void
UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
{
ssize_t ret = 0;
@@ -743,3 +759,27 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
warnx("ubx: config write fail");
}
+
+void
+UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
+{
+ struct ubx_header header;
+ uint8_t ck_a=0, ck_b=0;
+ header.sync1 = UBX_SYNC1;
+ header.sync2 = UBX_SYNC2;
+ header.msg_class = msg_class;
+ header.msg_id = msg_id;
+ header.length = size;
+
+ add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
+ add_checksum((uint8_t *)msg, size, ck_a, ck_b);
+
+ // Configure receive check
+ _clsID_needed = msg_class;
+ _msgID_needed = msg_id;
+
+ write(_fd, (const char *)&header, sizeof(header));
+ write(_fd, (const char *)msg, size);
+ write(_fd, (const char *)&ck_a, 1);
+ write(_fd, (const char *)&ck_b, 1);
+}
diff --git a/apps/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index e3dd1c7ea..5a433642c 100644
--- a/apps/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -65,26 +65,27 @@
#define UBX_MESSAGE_CFG_RATE 0x08
#define UBX_CFG_PRT_LENGTH 20
-#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
-#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
-#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
-#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
-#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
+#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
+#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
+#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
+#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
+#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
#define UBX_CFG_RATE_LENGTH 6
-#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */
+#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200 /**< 200ms for 5Hz */
#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */
#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */
#define UBX_CFG_NAV5_LENGTH 36
-#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */
+#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */
#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
-#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
+#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
#define UBX_CFG_MSG_LENGTH 8
#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10
#define UBX_MAX_PAYLOAD_LENGTH 500
@@ -92,6 +93,14 @@
/** the structures of the binary packets */
#pragma pack(push, 1)
+struct ubx_header {
+ uint8_t sync1;
+ uint8_t sync2;
+ uint8_t msg_class;
+ uint8_t msg_id;
+ uint16_t length;
+};
+
typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
int32_t lon; /**< Longitude * 1e-7, deg */
@@ -274,11 +283,17 @@ typedef struct {
uint16_t length;
uint8_t msgClass_payload;
uint8_t msgID_payload;
- uint8_t rate[6];
+ uint8_t rate;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_msg_packet_t;
+struct ubx_cfg_msg_rate {
+ uint8_t msg_class;
+ uint8_t msg_id;
+ uint8_t rate;
+};
+
// END the structures of the binary packets
// ************
@@ -341,55 +356,64 @@ class UBX : public GPS_Helper
public:
UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
~UBX();
- int receive(unsigned timeout);
- int configure(unsigned &baudrate);
+ int receive(unsigned timeout);
+ int configure(unsigned &baudrate);
private:
/**
* Parse the binary MTK packet
*/
- int parse_char(uint8_t b);
+ int parse_char(uint8_t b);
/**
* Handle the package once it has arrived
*/
- int handle_message(void);
+ int handle_message(void);
/**
* Reset the parse state machine for a fresh start
*/
- void decode_init(void);
+ void decode_init(void);
/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
- void add_byte_to_checksum(uint8_t);
+ void add_byte_to_checksum(uint8_t);
/**
* Add the two checksum bytes to an outgoing message
*/
- void add_checksum_to_message(uint8_t* message, const unsigned length);
+ void add_checksum_to_message(uint8_t* message, const unsigned length);
/**
* Helper to send a config packet
*/
- void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
+ void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
+
+ void configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
+
+ void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
+
+ void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
+
+ int wait_for_ack(unsigned timeout);
- int _fd;
+ int _fd;
struct vehicle_gps_position_s *_gps_position;
ubx_config_state_t _config_state;
- bool _waiting_for_ack;
- uint8_t _clsID_needed;
- uint8_t _msgID_needed;
+ bool _waiting_for_ack;
+ uint8_t _clsID_needed;
+ uint8_t _msgID_needed;
ubx_decode_state_t _decode_state;
- uint8_t _rx_buffer[RECV_BUFFER_SIZE];
- unsigned _rx_count;
- uint8_t _rx_ck_a;
- uint8_t _rx_ck_b;
- ubx_message_class_t _message_class;
+ uint8_t _rx_buffer[RECV_BUFFER_SIZE];
+ unsigned _rx_count;
+ uint8_t _rx_ck_a;
+ uint8_t _rx_ck_b;
+ ubx_message_class_t _message_class;
ubx_message_id_t _message_id;
- unsigned _payload_size;
+ unsigned _payload_size;
+ uint8_t _disable_cmd_counter;
};
#endif /* UBX_H_ */
diff --git a/apps/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index d9aa772d4..d9aa772d4 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk
new file mode 100644
index 000000000..f8895f5d5
--- /dev/null
+++ b/src/drivers/hil/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Hardware in the Loop (HIL) simulation actuator output bank
+#
+
+MODULE_COMMAND = hil
+
+SRCS = hil.cpp
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 8ab568282..78eda327c 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -75,7 +75,6 @@
* HMC5883 internal constants and data structures.
*/
-#define HMC5883L_BUS PX4_I2C_BUS_ONBOARD
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
/* Max measurement rate is 160Hz */
@@ -842,12 +841,25 @@ HMC5883::collect()
* 74 from all measurements centers them around zero.
*/
- /* to align the sensor axes with the board, x and y need to be flipped */
- _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
- /* flip axes and negate value for y */
- _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
- /* z remains z */
- _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+#ifdef PX4_I2C_BUS_ONBOARD
+ if (_bus == PX4_I2C_BUS_ONBOARD) {
+ /* to align the sensor axes with the board, x and y need to be flipped */
+ _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+ /* flip axes and negate value for y */
+ _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+ /* z remains z */
+ _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+ } else {
+#endif
+ /* XXX axis assignment of external sensor is yet unknown */
+ _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+ /* flip axes and negate value for y */
+ _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+ /* z remains z */
+ _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+#ifdef PX4_I2C_BUS_ONBOARD
+ }
+#endif
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
@@ -1211,15 +1223,27 @@ start()
if (g_dev != nullptr)
errx(1, "already started");
- /* create the driver */
- g_dev = new HMC5883(HMC5883L_BUS);
+ /* create the driver, attempt expansion bus first */
+ g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
+ if (g_dev != nullptr && OK != g_dev->init()) {
+ delete g_dev;
+ g_dev = nullptr;
+ }
+
+
+#ifdef PX4_I2C_BUS_ONBOARD
+ /* if this failed, attempt onboard sensor */
+ if (g_dev == nullptr) {
+ g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD);
+ if (g_dev != nullptr && OK != g_dev->init()) {
+ goto fail;
+ }
+ }
+#endif
if (g_dev == nullptr)
goto fail;
- if (OK != g_dev->init())
- goto fail;
-
/* set the poll rate to default, starts automatic data collection */
fd = open(MAG_DEVICE_PATH, O_RDONLY);
diff --git a/apps/drivers/hil/Makefile b/src/drivers/hmc5883/module.mk
index 1fb6e37bc..07377556d 100644
--- a/apps/drivers/hil/Makefile
+++ b/src/drivers/hmc5883/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,11 +32,12 @@
############################################################################
#
-# Interface driver for the PX4FMU board
+# HMC5883 driver
#
-APPNAME = hil
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+MODULE_COMMAND = hmc5883
-include $(APPDIR)/mk/app.mk
+# XXX seems excessive, check if 2048 is sufficient
+MODULE_STACKSIZE = 4096
+
+SRCS = hmc5883.cpp
diff --git a/apps/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c
index 31c9247aa..a13a6ef58 100644
--- a/apps/hott_telemetry/hott_telemetry_main.c
+++ b/src/drivers/hott_telemetry/hott_telemetry_main.c
@@ -1,7 +1,7 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: @author Simon Wilks <sjwilks@gmail.com>
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Author: Simon Wilks <sjwilks@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,6 +34,7 @@
/**
* @file hott_telemetry_main.c
+ * @author Simon Wilks <sjwilks@gmail.com>
*
* Graupner HoTT Telemetry implementation.
*
@@ -41,7 +42,6 @@
* a data packet can be returned if necessary.
*
* TODO: Add support for at least the vario and GPS sensor data.
- *
*/
#include <fcntl.h>
@@ -57,12 +57,6 @@
#include "messages.h"
-/* The following are equired for UART direct manipulation. */
-#include <arch/board/board.h>
-#include "up_arch.h"
-#include "chip.h"
-#include "stm32_internal.h"
-
static int thread_should_exit = false; /**< Deamon exit flag */
static int thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
diff --git a/apps/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c
index 8bfb99773..f0f32d244 100644
--- a/apps/hott_telemetry/messages.c
+++ b/src/drivers/hott_telemetry/messages.c
@@ -1,7 +1,7 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: @author Simon Wilks <sjwilks@gmail.com>
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Author: Simon Wilks <sjwilks@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,7 +34,7 @@
/**
* @file messages.c
- *
+ * @author Simon Wilks <sjwilks@gmail.com>
*/
#include "messages.h"
@@ -42,9 +42,11 @@
#include <string.h>
#include <systemlib/systemlib.h>
#include <unistd.h>
+#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
+static int airspeed_sub = -1;
static int battery_sub = -1;
static int sensor_sub = -1;
@@ -52,6 +54,7 @@ void messages_init(void)
{
battery_sub = orb_subscribe(ORB_ID(battery_status));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+ airspeed_sub = orb_subscribe(ORB_ID(airspeed));
}
void build_eam_response(uint8_t *buffer, int *size)
@@ -81,6 +84,15 @@ void build_eam_response(uint8_t *buffer, int *size)
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
+ /* get a local copy of the current sensor values */
+ struct airspeed_s airspeed;
+ memset(&airspeed, 0, sizeof(airspeed));
+ orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
+
+ uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
+ msg.speed_L = (uint8_t)speed & 0xff;
+ msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
+
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
diff --git a/apps/hott_telemetry/messages.h b/src/drivers/hott_telemetry/messages.h
index 44001e04f..dd38075fa 100644
--- a/apps/hott_telemetry/messages.h
+++ b/src/drivers/hott_telemetry/messages.h
@@ -1,7 +1,7 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: @author Simon Wilks <sjwilks@gmail.com>
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Author: Simon Wilks <sjwilks@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,6 +34,7 @@
/**
* @file messages.h
+ * @author Simon Wilks <sjwilks@gmail.com>
*
* Graupner HoTT Telemetry message generation.
*
diff --git a/apps/drivers/blinkm/Makefile b/src/drivers/hott_telemetry/module.mk
index 5a623693d..def1d59e9 100644
--- a/apps/drivers/blinkm/Makefile
+++ b/src/drivers/hott_telemetry/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,11 +32,10 @@
############################################################################
#
-# BlinkM I2C LED driver
+# Graupner HoTT Telemetry application.
#
-APPNAME = blinkm
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+MODULE_COMMAND = hott_telemetry
-include $(APPDIR)/mk/app.mk
+SRCS = hott_telemetry_main.c \
+ messages.c
diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 6227df72a..98098c83b 100644
--- a/apps/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,7 +32,8 @@
****************************************************************************/
/**
- * @file Driver for the ST L3GD20 MEMS gyro connected via SPI.
+ * @file l3gd20.cpp
+ * Driver for the ST L3GD20 MEMS gyro connected via SPI.
*/
#include <nuttx/config.h>
@@ -684,9 +685,10 @@ L3GD20::measure()
* 74 from all measurements centers them around zero.
*/
report->timestamp = hrt_absolute_time();
- /* XXX adjust for sensor alignment to board here */
- report->x_raw = raw_report.x;
- report->y_raw = raw_report.y;
+
+ /* swap x and y and negate y */
+ report->x_raw = raw_report.y;
+ report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
report->z_raw = raw_report.z;
report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk
new file mode 100644
index 000000000..23e77e871
--- /dev/null
+++ b/src/drivers/l3gd20/module.mk
@@ -0,0 +1,6 @@
+#
+# LSM303D accel/mag driver
+#
+
+MODULE_COMMAND = l3gd20
+SRCS = l3gd20.cpp
diff --git a/apps/drivers/led/led.cpp b/src/drivers/led/led.cpp
index c7c45525e..04b565358 100644
--- a/apps/drivers/led/led.cpp
+++ b/src/drivers/led/led.cpp
@@ -41,12 +41,17 @@
#include <drivers/device/device.h>
#include <drivers/drv_led.h>
-/* Ideally we'd be able to get these from up_internal.h */
-//#include <up_internal.h>
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
__BEGIN_DECLS
-extern void up_ledinit();
-extern void up_ledon(int led);
-extern void up_ledoff(int led);
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
__END_DECLS
class LED : device::CDev
@@ -74,7 +79,7 @@ int
LED::init()
{
CDev::init();
- up_ledinit();
+ led_init();
return 0;
}
@@ -86,11 +91,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case LED_ON:
- up_ledon(arg);
+ led_on(arg);
break;
case LED_OFF:
- up_ledoff(arg);
+ led_off(arg);
break;
default:
diff --git a/apps/systemlib/mixer/Makefile b/src/drivers/led/module.mk
index f8b02f194..777f3e442 100644
--- a/apps/systemlib/mixer/Makefile
+++ b/src/drivers/led/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,8 +32,7 @@
############################################################################
#
-# mixer library
+# Build the LED driver.
#
-LIBNAME = mixerlib
-include $(APPDIR)/mk/app.mk
+SRCS = led.cpp
diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 9d0f6bddc..397686e8b 100644
--- a/apps/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
diff --git a/apps/drivers/mb12xx/Makefile b/src/drivers/mb12xx/module.mk
index 0d2405787..4e00ada02 100644
--- a/apps/drivers/mb12xx/Makefile
+++ b/src/drivers/mb12xx/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2013 PX4 Development Team. All rights reserved.
+# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,8 +35,6 @@
# Makefile to build the Maxbotix Sonar driver.
#
-APPNAME = mb12xx
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+MODULE_COMMAND = mb12xx
-include $(APPDIR)/mk/app.mk
+SRCS = mb12xx.cpp
diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp
new file mode 100644
index 000000000..71932ad65
--- /dev/null
+++ b/src/drivers/md25/md25.cpp
@@ -0,0 +1,553 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file md25.cpp
+ *
+ * Driver for MD25 I2C Motor Driver
+ *
+ * references:
+ * http://www.robot-electronics.co.uk/htm/md25tech.htm
+ * http://www.robot-electronics.co.uk/files/rpi_md25.c
+ *
+ */
+
+#include "md25.hpp"
+#include <poll.h>
+#include <stdio.h>
+
+#include <systemlib/err.h>
+#include <arch/board/board.h>
+
+// registers
+enum {
+ // RW: read/write
+ // R: read
+ REG_SPEED1_RW = 0,
+ REG_SPEED2_RW,
+ REG_ENC1A_R,
+ REG_ENC1B_R,
+ REG_ENC1C_R,
+ REG_ENC1D_R,
+ REG_ENC2A_R,
+ REG_ENC2B_R,
+ REG_ENC2C_R,
+ REG_ENC2D_R,
+ REG_BATTERY_VOLTS_R,
+ REG_MOTOR1_CURRENT_R,
+ REG_MOTOR2_CURRENT_R,
+ REG_SW_VERSION_R,
+ REG_ACCEL_RATE_RW,
+ REG_MODE_RW,
+ REG_COMMAND_RW,
+};
+
+MD25::MD25(const char *deviceName, int bus,
+ uint16_t address, uint32_t speed) :
+ I2C("MD25", deviceName, bus, address, speed),
+ _controlPoll(),
+ _actuators(NULL, ORB_ID(actuator_controls_0), 20),
+ _version(0),
+ _motor1Speed(0),
+ _motor2Speed(0),
+ _revolutions1(0),
+ _revolutions2(0),
+ _batteryVoltage(0),
+ _motor1Current(0),
+ _motor2Current(0),
+ _motorAccel(0),
+ _mode(MODE_UNSIGNED_SPEED),
+ _command(CMD_RESET_ENCODERS)
+{
+ // setup control polling
+ _controlPoll.fd = _actuators.getHandle();
+ _controlPoll.events = POLLIN;
+
+ // if initialization fails raise an error, unless
+ // probing
+ int ret = I2C::init();
+
+ if (ret != OK) {
+ warnc(ret, "I2C::init failed for bus: %d address: %d\n", bus, address);
+ }
+
+ // setup default settings, reset encoders
+ setMotor1Speed(0);
+ setMotor2Speed(0);
+ resetEncoders();
+ _setMode(MD25::MODE_UNSIGNED_SPEED);
+ setSpeedRegulation(true);
+ setTimeout(true);
+}
+
+MD25::~MD25()
+{
+}
+
+int MD25::readData()
+{
+ uint8_t sendBuf[1];
+ sendBuf[0] = REG_SPEED1_RW;
+ uint8_t recvBuf[17];
+ int ret = transfer(sendBuf, sizeof(sendBuf),
+ recvBuf, sizeof(recvBuf));
+
+ if (ret == OK) {
+ _version = recvBuf[REG_SW_VERSION_R];
+ _motor1Speed = _uint8ToNorm(recvBuf[REG_SPEED1_RW]);
+ _motor2Speed = _uint8ToNorm(recvBuf[REG_SPEED2_RW]);
+ _revolutions1 = -int32_t((recvBuf[REG_ENC1A_R] << 24) +
+ (recvBuf[REG_ENC1B_R] << 16) +
+ (recvBuf[REG_ENC1C_R] << 8) +
+ recvBuf[REG_ENC1D_R]) / 360.0;
+ _revolutions2 = -int32_t((recvBuf[REG_ENC2A_R] << 24) +
+ (recvBuf[REG_ENC2B_R] << 16) +
+ (recvBuf[REG_ENC2C_R] << 8) +
+ recvBuf[REG_ENC2D_R]) / 360.0;
+ _batteryVoltage = recvBuf[REG_BATTERY_VOLTS_R] / 10.0;
+ _motor1Current = recvBuf[REG_MOTOR1_CURRENT_R] / 10.0;
+ _motor2Current = recvBuf[REG_MOTOR2_CURRENT_R] / 10.0;
+ _motorAccel = recvBuf[REG_ACCEL_RATE_RW];
+ _mode = e_mode(recvBuf[REG_MODE_RW]);
+ _command = e_cmd(recvBuf[REG_COMMAND_RW]);
+ }
+
+ return ret;
+}
+
+void MD25::status(char *string, size_t n)
+{
+ snprintf(string, n,
+ "version:\t%10d\n" \
+ "motor 1 speed:\t%10.2f\n" \
+ "motor 2 speed:\t%10.2f\n" \
+ "revolutions 1:\t%10.2f\n" \
+ "revolutions 2:\t%10.2f\n" \
+ "battery volts :\t%10.2f\n" \
+ "motor 1 current :\t%10.2f\n" \
+ "motor 2 current :\t%10.2f\n" \
+ "motor accel :\t%10d\n" \
+ "mode :\t%10d\n" \
+ "command :\t%10d\n",
+ getVersion(),
+ double(getMotor1Speed()),
+ double(getMotor2Speed()),
+ double(getRevolutions1()),
+ double(getRevolutions2()),
+ double(getBatteryVolts()),
+ double(getMotor1Current()),
+ double(getMotor2Current()),
+ getMotorAccel(),
+ getMode(),
+ getCommand());
+}
+
+uint8_t MD25::getVersion()
+{
+ return _version;
+}
+
+float MD25::getMotor1Speed()
+{
+ return _motor1Speed;
+}
+
+float MD25::getMotor2Speed()
+{
+ return _motor2Speed;
+}
+
+float MD25::getRevolutions1()
+{
+ return _revolutions1;
+}
+
+float MD25::getRevolutions2()
+{
+ return _revolutions2;
+}
+
+float MD25::getBatteryVolts()
+{
+ return _batteryVoltage;
+}
+
+float MD25::getMotor1Current()
+{
+ return _motor1Current;
+}
+float MD25::getMotor2Current()
+{
+ return _motor2Current;
+}
+
+uint8_t MD25::getMotorAccel()
+{
+ return _motorAccel;
+}
+
+MD25::e_mode MD25::getMode()
+{
+ return _mode;
+}
+
+MD25::e_cmd MD25::getCommand()
+{
+ return _command;
+}
+
+int MD25::resetEncoders()
+{
+ return _writeUint8(REG_COMMAND_RW,
+ CMD_RESET_ENCODERS);
+}
+
+int MD25::_setMode(e_mode mode)
+{
+ return _writeUint8(REG_MODE_RW,
+ mode);
+}
+
+int MD25::setSpeedRegulation(bool enable)
+{
+ if (enable) {
+ return _writeUint8(REG_COMMAND_RW,
+ CMD_ENABLE_SPEED_REGULATION);
+
+ } else {
+ return _writeUint8(REG_COMMAND_RW,
+ CMD_DISABLE_SPEED_REGULATION);
+ }
+}
+
+int MD25::setTimeout(bool enable)
+{
+ if (enable) {
+ return _writeUint8(REG_COMMAND_RW,
+ CMD_ENABLE_TIMEOUT);
+
+ } else {
+ return _writeUint8(REG_COMMAND_RW,
+ CMD_DISABLE_TIMEOUT);
+ }
+}
+
+int MD25::setDeviceAddress(uint8_t address)
+{
+ uint8_t sendBuf[1];
+ sendBuf[0] = CMD_CHANGE_I2C_SEQ_0;
+ int ret = OK;
+ ret = transfer(sendBuf, sizeof(sendBuf),
+ nullptr, 0);
+
+ if (ret != OK) {
+ warnc(ret, "MD25::setDeviceAddress");
+ return ret;
+ }
+
+ usleep(5000);
+ sendBuf[0] = CMD_CHANGE_I2C_SEQ_1;
+ ret = transfer(sendBuf, sizeof(sendBuf),
+ nullptr, 0);
+
+ if (ret != OK) {
+ warnc(ret, "MD25::setDeviceAddress");
+ return ret;
+ }
+
+ usleep(5000);
+ sendBuf[0] = CMD_CHANGE_I2C_SEQ_2;
+ ret = transfer(sendBuf, sizeof(sendBuf),
+ nullptr, 0);
+
+ if (ret != OK) {
+ warnc(ret, "MD25::setDeviceAddress");
+ return ret;
+ }
+
+ return OK;
+}
+
+int MD25::setMotor1Speed(float value)
+{
+ return _writeUint8(REG_SPEED1_RW,
+ _normToUint8(value));
+}
+
+int MD25::setMotor2Speed(float value)
+{
+ return _writeUint8(REG_SPEED2_RW,
+ _normToUint8(value));
+}
+
+void MD25::update()
+{
+ // wait for an actuator publication,
+ // check for exit condition every second
+ // note "::poll" is required to distinguish global
+ // poll from member function for driver
+ if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error
+
+ // if new data, send to motors
+ if (_actuators.updated()) {
+ _actuators.update();
+ setMotor1Speed(_actuators.control[CH_SPEED_LEFT]);
+ setMotor2Speed(_actuators.control[CH_SPEED_RIGHT]);
+ }
+}
+
+int MD25::probe()
+{
+ uint8_t goodAddress = 0;
+ bool found = false;
+ int ret = OK;
+
+ // try initial address first, if good, then done
+ if (readData() == OK) return ret;
+
+ // try all other addresses
+ uint8_t testAddress = 0;
+
+ //printf("searching for MD25 address\n");
+ while (true) {
+ set_address(testAddress);
+ ret = readData();
+
+ if (ret == OK && !found) {
+ //printf("device found at address: 0x%X\n", testAddress);
+ if (!found) {
+ found = true;
+ goodAddress = testAddress;
+ }
+ }
+
+ if (testAddress > 254) {
+ break;
+ }
+
+ testAddress++;
+ }
+
+ if (found) {
+ set_address(goodAddress);
+ return OK;
+
+ } else {
+ set_address(0);
+ return ret;
+ }
+}
+
+int MD25::search()
+{
+ uint8_t goodAddress = 0;
+ bool found = false;
+ int ret = OK;
+ // try all other addresses
+ uint8_t testAddress = 0;
+
+ //printf("searching for MD25 address\n");
+ while (true) {
+ set_address(testAddress);
+ ret = readData();
+
+ if (ret == OK && !found) {
+ printf("device found at address: 0x%X\n", testAddress);
+
+ if (!found) {
+ found = true;
+ goodAddress = testAddress;
+ }
+ }
+
+ if (testAddress > 254) {
+ break;
+ }
+
+ testAddress++;
+ }
+
+ if (found) {
+ set_address(goodAddress);
+ return OK;
+
+ } else {
+ set_address(0);
+ return ret;
+ }
+}
+
+int MD25::_writeUint8(uint8_t reg, uint8_t value)
+{
+ uint8_t sendBuf[2];
+ sendBuf[0] = reg;
+ sendBuf[1] = value;
+ return transfer(sendBuf, sizeof(sendBuf),
+ nullptr, 0);
+}
+
+int MD25::_writeInt8(uint8_t reg, int8_t value)
+{
+ uint8_t sendBuf[2];
+ sendBuf[0] = reg;
+ sendBuf[1] = value;
+ return transfer(sendBuf, sizeof(sendBuf),
+ nullptr, 0);
+}
+
+float MD25::_uint8ToNorm(uint8_t value)
+{
+ // TODO, should go from 0 to 255
+ // possibly should handle this differently
+ return (value - 128) / 127.0;
+}
+
+uint8_t MD25::_normToUint8(float value)
+{
+ if (value > 1) value = 1;
+
+ if (value < -1) value = -1;
+
+ // TODO, should go from 0 to 255
+ // possibly should handle this differently
+ return 127 * value + 128;
+}
+
+int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
+{
+ printf("md25 test: starting\n");
+
+ // setup
+ MD25 md25("/dev/md25", bus, address);
+
+ // print status
+ char buf[200];
+ md25.status(buf, sizeof(buf));
+ printf("%s\n", buf);
+
+ // setup for test
+ md25.setSpeedRegulation(true);
+ md25.setTimeout(true);
+ float dt = 0.1;
+ float speed = 0.2;
+ float t = 0;
+
+ // motor 1 test
+ printf("md25 test: spinning motor 1 forward for 1 rev at 0.1 speed\n");
+ t = 0;
+
+ while (true) {
+ t += dt;
+ md25.setMotor1Speed(speed);
+ md25.readData();
+ usleep(1000000 * dt);
+
+ if (md25.getRevolutions1() > 1) {
+ printf("finished 1 revolution fwd\n");
+ break;
+ }
+
+ if (t > 2.0f) break;
+ }
+
+ md25.setMotor1Speed(0);
+ printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1()));
+ md25.resetEncoders();
+
+ t = 0;
+
+ while (true) {
+ t += dt;
+ md25.setMotor1Speed(-speed);
+ md25.readData();
+ usleep(1000000 * dt);
+
+ if (md25.getRevolutions1() < -1) {
+ printf("finished 1 revolution rev\n");
+ break;
+ }
+
+ if (t > 2.0f) break;
+ }
+
+ md25.setMotor1Speed(0);
+ printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1()));
+ md25.resetEncoders();
+
+ // motor 2 test
+ printf("md25 test: spinning motor 2 forward for 1 rev at 0.1 speed\n");
+ t = 0;
+
+ while (true) {
+ t += dt;
+ md25.setMotor2Speed(speed);
+ md25.readData();
+ usleep(1000000 * dt);
+
+ if (md25.getRevolutions2() > 1) {
+ printf("finished 1 revolution fwd\n");
+ break;
+ }
+
+ if (t > 2.0f) break;
+ }
+
+ md25.setMotor2Speed(0);
+ printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2()));
+ md25.resetEncoders();
+
+ t = 0;
+
+ while (true) {
+ t += dt;
+ md25.setMotor2Speed(-speed);
+ md25.readData();
+ usleep(1000000 * dt);
+
+ if (md25.getRevolutions2() < -1) {
+ printf("finished 1 revolution rev\n");
+ break;
+ }
+
+ if (t > 2.0f) break;
+ }
+
+ md25.setMotor2Speed(0);
+ printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2()));
+ md25.resetEncoders();
+
+ printf("Test complete\n");
+ return 0;
+}
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp
new file mode 100644
index 000000000..e77511b16
--- /dev/null
+++ b/src/drivers/md25/md25.hpp
@@ -0,0 +1,293 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file md25.cpp
+ *
+ * Driver for MD25 I2C Motor Driver
+ *
+ * references:
+ * http://www.robot-electronics.co.uk/htm/md25tech.htm
+ * http://www.robot-electronics.co.uk/files/rpi_md25.c
+ *
+ */
+
+#pragma once
+
+#include <poll.h>
+#include <stdio.h>
+#include <controllib/block/UOrbSubscription.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <drivers/device/i2c.h>
+
+/**
+ * This is a driver for the MD25 motor controller utilizing the I2C interface.
+ */
+class MD25 : public device::I2C
+{
+public:
+
+ /**
+ * modes
+ *
+ * NOTE: this driver assumes we are always
+ * in mode 0!
+ *
+ * seprate speed mode:
+ * motor speed1 = speed1
+ * motor speed2 = speed2
+ * turn speed mode:
+ * motor speed1 = speed1 + speed2
+ * motor speed2 = speed2 - speed2
+ * unsigned:
+ * full rev (0), stop(128), full fwd (255)
+ * signed:
+ * full rev (-127), stop(0), full fwd (128)
+ *
+ * modes numbers:
+ * 0 : unsigned separate (default)
+ * 1 : signed separate
+ * 2 : unsigned turn
+ * 3 : signed turn
+ */
+ enum e_mode {
+ MODE_UNSIGNED_SPEED = 0,
+ MODE_SIGNED_SPEED,
+ MODE_UNSIGNED_SPEED_TURN,
+ MODE_SIGNED_SPEED_TURN,
+ };
+
+ /** commands */
+ enum e_cmd {
+ CMD_RESET_ENCODERS = 32,
+ CMD_DISABLE_SPEED_REGULATION = 48,
+ CMD_ENABLE_SPEED_REGULATION = 49,
+ CMD_DISABLE_TIMEOUT = 50,
+ CMD_ENABLE_TIMEOUT = 51,
+ CMD_CHANGE_I2C_SEQ_0 = 160,
+ CMD_CHANGE_I2C_SEQ_1 = 170,
+ CMD_CHANGE_I2C_SEQ_2 = 165,
+ };
+
+ /** control channels */
+ enum e_channels {
+ CH_SPEED_LEFT = 0,
+ CH_SPEED_RIGHT
+ };
+
+ /**
+ * constructor
+ * @param deviceName the name of the device e.g. "/dev/md25"
+ * @param bus the I2C bus
+ * @param address the adddress on the I2C bus
+ * @param speed the speed of the I2C communication
+ */
+ MD25(const char *deviceName,
+ int bus,
+ uint16_t address,
+ uint32_t speed = 100000);
+
+ /**
+ * deconstructor
+ */
+ virtual ~MD25();
+
+ /**
+ * @return software version
+ */
+ uint8_t getVersion();
+
+ /**
+ * @return speed of motor, normalized (-1, 1)
+ */
+ float getMotor1Speed();
+
+ /**
+ * @return speed of motor 2, normalized (-1, 1)
+ */
+ float getMotor2Speed();
+
+ /**
+ * @return number of rotations since reset
+ */
+ float getRevolutions1();
+
+ /**
+ * @return number of rotations since reset
+ */
+ float getRevolutions2();
+
+ /**
+ * @return battery voltage, volts
+ */
+ float getBatteryVolts();
+
+ /**
+ * @return motor 1 current, amps
+ */
+ float getMotor1Current();
+
+ /**
+ * @return motor 2 current, amps
+ */
+ float getMotor2Current();
+
+ /**
+ * @return the motor acceleration
+ * controls motor speed change (1-10)
+ * accel rate | time for full fwd. to full rev.
+ * 1 | 6.375 s
+ * 2 | 1.6 s
+ * 3 | 0.675 s
+ * 5(default) | 1.275 s
+ * 10 | 0.65 s
+ */
+ uint8_t getMotorAccel();
+
+ /**
+ * @return motor output mode
+ * */
+ e_mode getMode();
+
+ /**
+ * @return current command register value
+ */
+ e_cmd getCommand();
+
+ /**
+ * resets the encoders
+ * @return non-zero -> error
+ * */
+ int resetEncoders();
+
+ /**
+ * enable/disable speed regulation
+ * @return non-zero -> error
+ */
+ int setSpeedRegulation(bool enable);
+
+ /**
+ * set the timeout for the motors
+ * enable/disable timeout (motor stop)
+ * after 2 sec of no i2c messages
+ * @return non-zero -> error
+ */
+ int setTimeout(bool enable);
+
+ /**
+ * sets the device address
+ * can only be done with one MD25
+ * on the bus
+ * @return non-zero -> error
+ */
+ int setDeviceAddress(uint8_t address);
+
+ /**
+ * set motor 1 speed
+ * @param normSpeed normalize speed between -1 and 1
+ * @return non-zero -> error
+ */
+ int setMotor1Speed(float normSpeed);
+
+ /**
+ * set motor 2 speed
+ * @param normSpeed normalize speed between -1 and 1
+ * @return non-zero -> error
+ */
+ int setMotor2Speed(float normSpeed);
+
+ /**
+ * main update loop that updates MD25 motor
+ * speeds based on actuator publication
+ */
+ void update();
+
+ /**
+ * probe for device
+ */
+ virtual int probe();
+
+ /**
+ * search for device
+ */
+ int search();
+
+ /**
+ * read data from i2c
+ */
+ int readData();
+
+ /**
+ * print status
+ */
+ void status(char *string, size_t n);
+
+private:
+ /** poll structure for control packets */
+ struct pollfd _controlPoll;
+
+ /** actuator controls subscription */
+ control::UOrbSubscription<actuator_controls_s> _actuators;
+
+ // local copy of data from i2c device
+ uint8_t _version;
+ float _motor1Speed;
+ float _motor2Speed;
+ float _revolutions1;
+ float _revolutions2;
+ float _batteryVoltage;
+ float _motor1Current;
+ float _motor2Current;
+ uint8_t _motorAccel;
+ e_mode _mode;
+ e_cmd _command;
+
+ // private methods
+ int _writeUint8(uint8_t reg, uint8_t value);
+ int _writeInt8(uint8_t reg, int8_t value);
+ float _uint8ToNorm(uint8_t value);
+ uint8_t _normToUint8(float value);
+
+ /**
+ * set motor control mode,
+ * this driver assumed we are always in mode 0
+ * so we don't let the user change the mode
+ * @return non-zero -> error
+ */
+ int _setMode(e_mode);
+};
+
+// unit testing
+int md25Test(const char *deviceName, uint8_t bus, uint8_t address);
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp
new file mode 100644
index 000000000..80850e708
--- /dev/null
+++ b/src/drivers/md25/md25_main.cpp
@@ -0,0 +1,264 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+
+/**
+ * @ file md25.cpp
+ *
+ * Driver for MD25 I2C Motor Driver
+ *
+ * references:
+ * http://www.robot-electronics.co.uk/htm/md25tech.htm
+ * http://www.robot-electronics.co.uk/files/rpi_md25.c
+ *
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+
+#include <arch/board/board.h>
+#include "md25.hpp"
+
+static bool thread_should_exit = false; /**< Deamon exit flag */
+static bool thread_running = false; /**< Deamon status flag */
+static int deamon_task; /**< Handle of deamon task / thread */
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int md25_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int md25_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+ if (reason)
+ fprintf(stderr, "%s\n", reason);
+
+ fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n");
+ exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int md25_main(int argc, char *argv[])
+{
+
+ if (argc < 1)
+ usage("missing command");
+
+ if (!strcmp(argv[1], "start")) {
+
+ if (thread_running) {
+ printf("md25 already running\n");
+ /* this is not an error */
+ exit(0);
+ }
+
+ thread_should_exit = false;
+ deamon_task = task_spawn("md25",
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 10,
+ 2048,
+ md25_thread_main,
+ (const char **)argv);
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "test")) {
+
+ if (argc < 4) {
+ printf("usage: md25 test bus address\n");
+ exit(0);
+ }
+
+ const char *deviceName = "/dev/md25";
+
+ uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+ uint8_t address = strtoul(argv[3], nullptr, 0);
+
+ md25Test(deviceName, bus, address);
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "probe")) {
+ if (argc < 4) {
+ printf("usage: md25 probe bus address\n");
+ exit(0);
+ }
+
+ const char *deviceName = "/dev/md25";
+
+ uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+ uint8_t address = strtoul(argv[3], nullptr, 0);
+
+ MD25 md25(deviceName, bus, address);
+
+ int ret = md25.probe();
+
+ if (ret == OK) {
+ printf("MD25 found on bus %d at address 0x%X\n", bus, md25.get_address());
+
+ } else {
+ printf("MD25 not found on bus %d\n", bus);
+ }
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "search")) {
+ if (argc < 3) {
+ printf("usage: md25 search bus\n");
+ exit(0);
+ }
+
+ const char *deviceName = "/dev/md25";
+
+ uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+ uint8_t address = strtoul(argv[3], nullptr, 0);
+
+ MD25 md25(deviceName, bus, address);
+
+ md25.search();
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "change_address")) {
+ if (argc < 5) {
+ printf("usage: md25 change_address bus old_address new_address\n");
+ exit(0);
+ }
+
+ const char *deviceName = "/dev/md25";
+
+ uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+ uint8_t old_address = strtoul(argv[3], nullptr, 0);
+
+ uint8_t new_address = strtoul(argv[4], nullptr, 0);
+
+ MD25 md25(deviceName, bus, old_address);
+
+ int ret = md25.setDeviceAddress(new_address);
+
+ if (ret == OK) {
+ printf("MD25 new address set to 0x%X\n", new_address);
+
+ } else {
+ printf("MD25 failed to set address to 0x%X\n", new_address);
+ }
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "stop")) {
+ thread_should_exit = true;
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "status")) {
+ if (thread_running) {
+ printf("\tmd25 app is running\n");
+
+ } else {
+ printf("\tmd25 app not started\n");
+ }
+
+ exit(0);
+ }
+
+ usage("unrecognized command");
+ exit(1);
+}
+
+int md25_thread_main(int argc, char *argv[])
+{
+ printf("[MD25] starting\n");
+
+ if (argc < 5) {
+ // extra md25 in arg list since this is a thread
+ printf("usage: md25 start bus address\n");
+ exit(0);
+ }
+
+ const char *deviceName = "/dev/md25";
+
+ uint8_t bus = strtoul(argv[3], nullptr, 0);
+
+ uint8_t address = strtoul(argv[4], nullptr, 0);
+
+ // start
+ MD25 md25("/dev/md25", bus, address);
+
+ thread_running = true;
+
+ // loop
+ while (!thread_should_exit) {
+ md25.update();
+ }
+
+ // exit
+ printf("[MD25] exiting.\n");
+ thread_running = false;
+ return 0;
+}
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/apps/ardrone_interface/Makefile b/src/drivers/md25/module.mk
index fea96082f..13821a6b5 100644
--- a/apps/ardrone_interface/Makefile
+++ b/src/drivers/md25/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,11 +32,11 @@
############################################################################
#
-# Makefile to build ardrone interface
+# MD25 I2C Based Motor Controller
+# http://www.robot-electronics.co.uk/htm/md25tech.htm
#
-APPNAME = ardrone_interface
-PRIORITY = SCHED_PRIORITY_MAX - 15
-STACKSIZE = 2048
+MODULE_COMMAND = md25
-include $(APPDIR)/mk/app.mk
+SRCS = md25.cpp \
+ md25_main.cpp
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
new file mode 100644
index 000000000..3a735e26f
--- /dev/null
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -0,0 +1,1442 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mkblctrl.cpp
+ *
+ * Driver/configurator for the Mikrokopter BL-Ctrl.
+ * Marco Bauer
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/i2c.h>
+
+#include <drivers/device/device.h>
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_gpio.h>
+#include <drivers/boards/px4fmu/px4fmu_internal.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_rc_input.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+#include <drivers/drv_mixer.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_outputs.h>
+
+#include <systemlib/err.h>
+#include <systemlib/ppm_decode.h>
+
+#define I2C_BUS_SPEED 400000
+#define UPDATE_RATE 400
+#define MAX_MOTORS 8
+#define BLCTRL_BASE_ADDR 0x29
+#define BLCTRL_OLD 0
+#define BLCTRL_NEW 1
+#define BLCTRL_MIN_VALUE -0.920F
+#define MOTOR_STATE_PRESENT_MASK 0x80
+#define MOTOR_STATE_ERROR_MASK 0x7F
+#define MOTOR_SPINUP_COUNTER 2500
+
+
+class MK : public device::I2C
+{
+public:
+ enum Mode {
+ MODE_2PWM,
+ MODE_4PWM,
+ MODE_NONE
+ };
+
+ enum MappingMode {
+ MAPPING_MK = 0,
+ MAPPING_PX4,
+ };
+
+ enum FrameType {
+ FRAME_PLUS = 0,
+ FRAME_X,
+ };
+
+ MK(int bus);
+ ~MK();
+
+ virtual int ioctl(file *filp, int cmd, unsigned long arg);
+ virtual int init(unsigned motors);
+
+ int set_mode(Mode mode);
+ int set_pwm_rate(unsigned rate);
+ int set_motor_count(unsigned count);
+ int set_motor_test(bool motortest);
+ int set_px4mode(int px4mode);
+ int set_frametype(int frametype);
+ unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput);
+
+private:
+ static const unsigned _max_actuators = MAX_MOTORS;
+ static const bool showDebug = false;
+
+ Mode _mode;
+ int _update_rate;
+ int _current_update_rate;
+ int _task;
+ int _t_actuators;
+ int _t_armed;
+ unsigned int _motor;
+ int _px4mode;
+ int _frametype;
+ orb_advert_t _t_outputs;
+ orb_advert_t _t_actuators_effective;
+ unsigned int _num_outputs;
+ bool _primary_pwm_device;
+ bool _motortest;
+
+ volatile bool _task_should_exit;
+ bool _armed;
+
+ unsigned long debugCounter;
+
+ MixerGroup *_mixers;
+
+ actuator_controls_s _controls;
+
+ static void task_main_trampoline(int argc, char *argv[]);
+ void task_main() __attribute__((noreturn));
+
+ static int control_callback(uintptr_t handle,
+ uint8_t control_group,
+ uint8_t control_index,
+ float &input);
+
+ int pwm_ioctl(file *filp, int cmd, unsigned long arg);
+
+ struct GPIOConfig {
+ uint32_t input;
+ uint32_t output;
+ uint32_t alt;
+ };
+
+ static const GPIOConfig _gpio_tab[];
+ static const unsigned _ngpio;
+
+ void gpio_reset(void);
+ void gpio_set_function(uint32_t gpios, int function);
+ void gpio_write(uint32_t gpios, int function);
+ uint32_t gpio_read(void);
+ int gpio_ioctl(file *filp, int cmd, unsigned long arg);
+ int mk_servo_arm(bool status);
+
+ int mk_servo_set(unsigned int chan, float val);
+ int mk_servo_set_test(unsigned int chan, float val);
+ int mk_servo_test(unsigned int chan);
+
+
+};
+
+const MK::GPIOConfig MK::_gpio_tab[] = {
+ {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
+ {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
+ {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
+ {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
+ {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
+ {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
+ {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
+ {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
+};
+
+const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]);
+
+const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad + configuration
+const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 }; // Addresstranslator for Hexa + configuration
+const int blctrlAddr_octo_plus[] = { 0, 3, -1, 0, 3, 0, 0, -5 }; // Addresstranslator for Octo + configuration
+
+const int blctrlAddr_quad_x[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad X configuration
+const int blctrlAddr_hexa_x[] = { 2, 4, -2, 0, -3, -1, 0, 0 }; // Addresstranslator for Hexa X configuration
+const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 }; // Addresstranslator for Octo X configuration
+
+const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0};
+
+int addrTranslator[] = {0,0,0,0,0,0,0,0};
+
+struct MotorData_t
+{
+ unsigned int Version; // the version of the BL (0 = old)
+ unsigned int SetPoint; // written by attitude controller
+ unsigned int SetPointLowerBits; // for higher Resolution of new BLs
+ unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present
+ unsigned int ReadMode; // select data to read
+ // the following bytes must be exactly in that order!
+ unsigned int Current; // in 0.1 A steps, read back from BL
+ unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit
+ unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in
+ unsigned int RoundCount;
+};
+
+MotorData_t Motor[MAX_MOTORS];
+
+
+namespace
+{
+
+MK *g_mk;
+
+} // namespace
+
+MK::MK(int bus) :
+ I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
+ _mode(MODE_NONE),
+ _update_rate(50),
+ _task(-1),
+ _t_actuators(-1),
+ _t_armed(-1),
+ _t_outputs(0),
+ _t_actuators_effective(0),
+ _num_outputs(0),
+ _motortest(false),
+ _motor(-1),
+ _px4mode(MAPPING_MK),
+ _frametype(FRAME_PLUS),
+ _primary_pwm_device(false),
+ _task_should_exit(false),
+ _armed(false),
+ _mixers(nullptr)
+{
+ _debug_enabled = true;
+}
+
+MK::~MK()
+{
+ if (_task != -1) {
+ /* tell the task we want it to go away */
+ _task_should_exit = true;
+
+ unsigned i = 10;
+
+ do {
+ /* wait 50ms - it should wake every 100ms or so worst-case */
+ usleep(50000);
+
+ /* if we have given up, kill it */
+ if (--i == 0) {
+ task_delete(_task);
+ break;
+ }
+
+ } while (_task != -1);
+ }
+
+ /* clean up the alternate device node */
+ if (_primary_pwm_device)
+ unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+ g_mk = nullptr;
+}
+
+int
+MK::init(unsigned motors)
+{
+ _num_outputs = motors;
+ debugCounter = 0;
+ int ret;
+ ASSERT(_task == -1);
+
+ ret = I2C::init();
+
+ if (ret != OK) {
+ warnx("I2C init failed");
+ return ret;
+ }
+
+ usleep(500000);
+
+ /* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+
+ if (ret == OK) {
+ log("default PWM output device");
+ _primary_pwm_device = true;
+ }
+
+ /* reset GPIOs */
+ gpio_reset();
+
+ /* start the IO interface task */
+ _task = task_spawn("mkblctrl",
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX -20,
+ 2048,
+ (main_t)&MK::task_main_trampoline,
+ nullptr);
+
+
+ if (_task < 0) {
+ debug("task start failed: %d", errno);
+ return -errno;
+ }
+
+ return OK;
+}
+
+void
+MK::task_main_trampoline(int argc, char *argv[])
+{
+ g_mk->task_main();
+}
+
+int
+MK::set_mode(Mode mode)
+{
+ /*
+ * Configure for PWM output.
+ *
+ * Note that regardless of the configured mode, the task is always
+ * listening and mixing; the mode just selects which of the channels
+ * are presented on the output pins.
+ */
+ switch (mode) {
+ case MODE_2PWM:
+ if(_num_outputs == 4) {
+ //debug("MODE_QUAD");
+ } else if(_num_outputs == 6) {
+ //debug("MODE_HEXA");
+ } else if(_num_outputs == 8) {
+ //debug("MODE_OCTO");
+ }
+ //up_pwm_servo_init(0x3);
+ up_pwm_servo_deinit();
+ _update_rate = UPDATE_RATE; /* default output rate */
+ break;
+
+ case MODE_4PWM:
+ if(_num_outputs == 4) {
+ //debug("MODE_QUADRO");
+ } else if(_num_outputs == 6) {
+ //debug("MODE_HEXA");
+ } else if(_num_outputs == 8) {
+ //debug("MODE_OCTO");
+ }
+ //up_pwm_servo_init(0xf);
+ up_pwm_servo_deinit();
+ _update_rate = UPDATE_RATE; /* default output rate */
+ break;
+
+ case MODE_NONE:
+ debug("MODE_NONE");
+ /* disable servo outputs and set a very low update rate */
+ up_pwm_servo_deinit();
+ _update_rate = UPDATE_RATE;
+ break;
+
+ default:
+ return -EINVAL;
+ }
+
+ _mode = mode;
+ return OK;
+}
+
+int
+MK::set_pwm_rate(unsigned rate)
+{
+ if ((rate > 500) || (rate < 10))
+ return -EINVAL;
+
+ _update_rate = rate;
+ return OK;
+}
+
+int
+MK::set_px4mode(int px4mode)
+{
+ _px4mode = px4mode;
+}
+
+int
+MK::set_frametype(int frametype)
+{
+ _frametype = frametype;
+}
+
+
+int
+MK::set_motor_count(unsigned count)
+{
+ if(count > 0) {
+
+ _num_outputs = count;
+
+ if(_px4mode == MAPPING_MK) {
+ if(_frametype == FRAME_PLUS) {
+ fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n");
+ } else if(_frametype == FRAME_X) {
+ fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n");
+ }
+ if(_num_outputs == 4) {
+ if(_frametype == FRAME_PLUS) {
+ memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus));
+ } else if(_frametype == FRAME_X) {
+ memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x));
+ }
+ } else if(_num_outputs == 6) {
+ if(_frametype == FRAME_PLUS) {
+ memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus));
+ } else if(_frametype == FRAME_X) {
+ memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x));
+ }
+ } else if(_num_outputs == 8) {
+ if(_frametype == FRAME_PLUS) {
+ memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus));
+ } else if(_frametype == FRAME_X) {
+ memcpy(&addrTranslator, &blctrlAddr_octo_x, sizeof(blctrlAddr_octo_x));
+ }
+ }
+ } else {
+ fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n");
+ memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4));
+ }
+
+ if(_num_outputs == 4) {
+ fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n");
+ } else if(_num_outputs == 6) {
+ fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n");
+ } else if(_num_outputs == 8) {
+ fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n");
+ }
+
+ return OK;
+
+ } else {
+ return -1;
+ }
+
+}
+
+int
+MK::set_motor_test(bool motortest)
+{
+ _motortest = motortest;
+ return OK;
+}
+
+
+void
+MK::task_main()
+{
+ /*
+ * Subscribe to the appropriate PWM output topic based on whether we are the
+ * primary PWM output or not.
+ */
+ _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+ ORB_ID(actuator_controls_1));
+ /* force a reset of the update rate */
+ _current_update_rate = 0;
+
+ _t_armed = orb_subscribe(ORB_ID(actuator_armed));
+ orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+
+ /* advertise the mixed control outputs */
+ actuator_outputs_s outputs;
+ memset(&outputs, 0, sizeof(outputs));
+ /* advertise the mixed control outputs */
+ _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+ &outputs);
+
+ /* advertise the effective control inputs */
+ actuator_controls_effective_s controls_effective;
+ memset(&controls_effective, 0, sizeof(controls_effective));
+ /* advertise the effective control inputs */
+ _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
+ &controls_effective);
+
+ pollfd fds[2];
+ fds[0].fd = _t_actuators;
+ fds[0].events = POLLIN;
+ fds[1].fd = _t_armed;
+ fds[1].events = POLLIN;
+
+ // rc input, published to ORB
+ struct rc_input_values rc_in;
+ orb_advert_t to_input_rc = 0;
+
+ memset(&rc_in, 0, sizeof(rc_in));
+ rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
+
+ log("starting");
+ long update_rate_in_us = 0;
+
+ /* loop until killed */
+ while (!_task_should_exit) {
+
+ /* handle update rate changes */
+ if (_current_update_rate != _update_rate) {
+ int update_rate_in_ms = int(1000 / _update_rate);
+ update_rate_in_us = long(1000000 / _update_rate);
+
+ /* reject faster than 500 Hz updates */
+ if (update_rate_in_ms < 2) {
+ update_rate_in_ms = 2;
+ _update_rate = 500;
+ }
+ /* reject slower than 50 Hz updates */
+ if (update_rate_in_ms > 20) {
+ update_rate_in_ms = 20;
+ _update_rate = 50;
+ }
+
+ orb_set_interval(_t_actuators, update_rate_in_ms);
+ up_pwm_servo_set_rate(_update_rate);
+ _current_update_rate = _update_rate;
+ }
+
+ /* sleep waiting for data, but no more than a second */
+ int ret = ::poll(&fds[0], 2, 1000);
+
+ /* this would be bad... */
+ if (ret < 0) {
+ log("poll error %d", errno);
+ usleep(1000000);
+ continue;
+ }
+
+ /* do we have a control update? */
+ if (fds[0].revents & POLLIN) {
+
+ /* get controls - must always do this to avoid spinning */
+ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
+
+ /* can we mix? */
+ if (_mixers != nullptr) {
+
+ /* do mixing */
+ outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
+ outputs.timestamp = hrt_absolute_time();
+
+ // XXX output actual limited values
+ memcpy(&controls_effective, &_controls, sizeof(controls_effective));
+
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
+
+ /* iterate actuators */
+ for (unsigned int i = 0; i < _num_outputs; i++) {
+
+ /* last resort: catch NaN, INF and out-of-band errors */
+ if (i < outputs.noutputs &&
+ isfinite(outputs.output[i]) &&
+ outputs.output[i] >= -1.0f &&
+ outputs.output[i] <= 1.0f) {
+ /* scale for PWM output 900 - 2100us */
+ //outputs.output[i] = 1500 + (600 * outputs.output[i]);
+ //outputs.output[i] = 127 + (127 * outputs.output[i]);
+ } else {
+ /*
+ * Value is NaN, INF or out of band - set to the minimum value.
+ * This will be clearly visible on the servo status and will limit the risk of accidentally
+ * spinning motors. It would be deadly in flight.
+ */
+ if(outputs.output[i] < -1.0f) {
+ outputs.output[i] = -1.0f;
+ } else if(outputs.output[i] > 1.0f) {
+ outputs.output[i] = 1.0f;
+ } else {
+ outputs.output[i] = -1.0f;
+ }
+ }
+
+ /* don't go under BLCTRL_MIN_VALUE */
+ if(outputs.output[i] < BLCTRL_MIN_VALUE) {
+ outputs.output[i] = BLCTRL_MIN_VALUE;
+ }
+ //_motortest = true;
+ /* output to BLCtrl's */
+ if(_motortest == true) {
+ mk_servo_test(i);
+ } else {
+ //mk_servo_set(i, outputs.output[i]);
+ mk_servo_set_test(i, outputs.output[i]); // 8Bit
+ }
+
+
+ }
+
+ /* and publish for anyone that cares to see */
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+ }
+ }
+
+ /* how about an arming update? */
+ if (fds[1].revents & POLLIN) {
+ actuator_armed_s aa;
+
+ /* get new value */
+ orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+
+ /* update PWM servo armed status if armed and not locked down */
+ ////up_pwm_servo_arm(aa.armed && !aa.lockdown);
+ mk_servo_arm(aa.armed && !aa.lockdown);
+ }
+
+ // see if we have new PPM input data
+ if (ppm_last_valid_decode != rc_in.timestamp) {
+ // we have a new PPM frame. Publish it.
+ rc_in.channel_count = ppm_decoded_channels;
+ if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
+ rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
+ }
+ for (uint8_t i=0; i<rc_in.channel_count; i++) {
+ rc_in.values[i] = ppm_buffer[i];
+ }
+ rc_in.timestamp = ppm_last_valid_decode;
+
+ /* lazily advertise on first publication */
+ if (to_input_rc == 0) {
+ to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
+ } else {
+ orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
+ }
+ }
+
+ }
+
+ ::close(_t_actuators);
+ ::close(_t_actuators_effective);
+ ::close(_t_armed);
+
+
+ /* make sure servos are off */
+ up_pwm_servo_deinit();
+
+ log("stopping");
+
+ /* note - someone else is responsible for restoring the GPIO config */
+
+ /* tell the dtor that we are exiting */
+ _task = -1;
+ _exit(0);
+}
+
+
+int
+MK::mk_servo_arm(bool status)
+{
+ _armed = status;
+ return 0;
+}
+
+
+unsigned int
+MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
+{
+ _retries = 50;
+ uint8_t foundMotorCount = 0;
+
+ for(unsigned i=0; i<MAX_MOTORS; i++) {
+ Motor[i].Version = 0;
+ Motor[i].SetPoint = 0;
+ Motor[i].SetPointLowerBits = 0;
+ Motor[i].State = 0;
+ Motor[i].ReadMode = 0;
+ Motor[i].Current = 0;
+ Motor[i].MaxPWM = 0;
+ Motor[i].Temperature = 0;
+ Motor[i].RoundCount = 0;
+ }
+
+ uint8_t msg = 0;
+ uint8_t result[3];
+
+ for(unsigned i=0; i< count; i++) {
+ result[0] = 0;
+ result[1] = 0;
+ result[2] = 0;
+
+ set_address( BLCTRL_BASE_ADDR + i );
+
+ if (OK == transfer(&msg, 1, &result[0], 3)) {
+ Motor[i].Current = result[0];
+ Motor[i].MaxPWM = result[1];
+ Motor[i].Temperature = result[2];
+ Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
+ foundMotorCount++;
+ if(Motor[i].MaxPWM == 250) {
+ Motor[i].Version = BLCTRL_NEW;
+ } else {
+ Motor[i].Version = BLCTRL_OLD;
+ }
+ }
+ }
+
+ if(showOutput) {
+ fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount);
+ for(unsigned i=0; i< foundMotorCount; i++) {
+ fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i,Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
+ }
+
+ if(foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
+ _task_should_exit = true;
+ }
+ }
+
+ return foundMotorCount;
+}
+
+
+
+
+int
+MK::mk_servo_set(unsigned int chan, float val)
+{
+ float tmpVal = 0;
+ _retries = 0;
+ uint8_t result[3] = { 0,0,0 };
+ uint8_t msg[2] = { 0,0 };
+ uint8_t rod=0;
+ uint8_t bytesToSendBL2 = 2;
+
+
+ tmpVal = (1023 + (1023 * val));
+ if(tmpVal > 2047) {
+ tmpVal = 2047;
+ }
+
+
+ Motor[chan].SetPoint = (uint8_t) tmpVal / 3; // divide 8
+ Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 8; // rest of divide 8
+ //rod = (uint8_t) tmpVal % 8;
+ //Motor[chan].SetPointLowerBits = rod<<1; // rest of divide 8
+ Motor[chan].SetPointLowerBits = 0;
+
+ if(_armed == false) {
+ Motor[chan].SetPoint = 0;
+ Motor[chan].SetPointLowerBits = 0;
+ }
+
+ //if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) {
+ set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+
+ if(Motor[chan].Version == BLCTRL_OLD) {
+ /*
+ * Old BL-Ctrl 8Bit served. Version < 2.0
+ */
+ msg[0] = Motor[chan].SetPoint;
+ if(Motor[chan].RoundCount >= 16) {
+ // on each 16th cyle we read out the status messages from the blctrl
+ if (OK == transfer(&msg[0], 1, &result[0], 2)) {
+ Motor[chan].Current = result[0];
+ Motor[chan].MaxPWM = result[1];
+ Motor[chan].Temperature = 255;;
+ } else {
+ if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
+ }
+ Motor[chan].RoundCount = 0;
+ } else {
+ if (OK != transfer(&msg[0], 1, nullptr, 0)) {
+ if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
+ }
+ }
+
+ } else {
+ /*
+ * New BL-Ctrl 11Bit served. Version >= 2.0
+ */
+ msg[0] = Motor[chan].SetPoint;
+ msg[1] = Motor[chan].SetPointLowerBits;
+
+ if(Motor[chan].SetPointLowerBits == 0) {
+ bytesToSendBL2 = 1; // if setpoint lower bits are zero, we send only the higher bits - this saves time
+ }
+
+ if(Motor[chan].RoundCount >= 16) {
+ // on each 16th cyle we read out the status messages from the blctrl
+ if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) {
+ Motor[chan].Current = result[0];
+ Motor[chan].MaxPWM = result[1];
+ Motor[chan].Temperature = result[2];
+ } else {
+ if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
+ }
+ Motor[chan].RoundCount = 0;
+ } else {
+ if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) {
+ if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error
+ }
+ }
+
+ }
+
+ Motor[chan].RoundCount++;
+ //}
+
+ if(showDebug == true) {
+ debugCounter++;
+ if(debugCounter == 2000) {
+ debugCounter = 0;
+ for(int i=0; i<_num_outputs; i++){
+ if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
+ fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State);
+ }
+ }
+ fprintf(stderr, "\n");
+ }
+ }
+ return 0;
+}
+
+int
+MK::mk_servo_set_test(unsigned int chan, float val)
+{
+ _retries = 0;
+ int ret;
+
+ float tmpVal = 0;
+
+ uint8_t msg[2] = { 0,0 };
+
+ tmpVal = (1023 + (1023 * val));
+ if(tmpVal > 2048) {
+ tmpVal = 2048;
+ }
+
+ Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
+
+ if(_armed == false) {
+ Motor[chan].SetPoint = 0;
+ Motor[chan].SetPointLowerBits = 0;
+ }
+
+ msg[0] = Motor[chan].SetPoint;
+
+ set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+ ret = transfer(&msg[0], 1, nullptr, 0);
+
+ ret = OK;
+
+ return ret;
+}
+
+
+int
+MK::mk_servo_test(unsigned int chan)
+{
+ int ret=0;
+ float tmpVal = 0;
+ float val = -1;
+ _retries = 0;
+ uint8_t msg[2] = { 0,0 };
+
+ if(debugCounter >= MOTOR_SPINUP_COUNTER) {
+ debugCounter = 0;
+ _motor++;
+
+ if(_motor < _num_outputs) {
+ fprintf(stderr, "[mkblctrl] Motortest - #%i:\tspinup\n", _motor);
+ }
+
+ if(_motor >= _num_outputs) {
+ _motor = -1;
+ _motortest = false;
+ }
+
+ }
+ debugCounter++;
+
+ if(_motor == chan) {
+ val = BLCTRL_MIN_VALUE;
+ } else {
+ val = -1;
+ }
+
+ tmpVal = (1023 + (1023 * val));
+ if(tmpVal > 2048) {
+ tmpVal = 2048;
+ }
+
+ //Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
+ //Motor[chan].SetPointLowerBits = (uint8_t) (tmpVal % 8) & 0x07;
+ Motor[chan].SetPoint = (uint8_t) tmpVal>>3;
+ Motor[chan].SetPointLowerBits = (uint8_t) tmpVal & 0x07;
+
+ if(_motor != chan) {
+ Motor[chan].SetPoint = 0;
+ Motor[chan].SetPointLowerBits = 0;
+ }
+
+ if(Motor[chan].Version == BLCTRL_OLD) {
+ msg[0] = Motor[chan].SetPoint;
+ } else {
+ msg[0] = Motor[chan].SetPoint;
+ msg[1] = Motor[chan].SetPointLowerBits;
+ }
+
+ set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+ if(Motor[chan].Version == BLCTRL_OLD) {
+ ret = transfer(&msg[0], 1, nullptr, 0);
+ } else {
+ ret = transfer(&msg[0], 2, nullptr, 0);
+ }
+
+ return ret;
+}
+
+
+int
+MK::control_callback(uintptr_t handle,
+ uint8_t control_group,
+ uint8_t control_index,
+ float &input)
+{
+ const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+ input = controls->control[control_index];
+ return 0;
+}
+
+int
+MK::ioctl(file *filp, int cmd, unsigned long arg)
+{
+ int ret;
+
+ // XXX disabled, confusing users
+ //debug("ioctl 0x%04x 0x%08x", cmd, arg);
+
+ /* try it as a GPIO ioctl first */
+ ret = gpio_ioctl(filp, cmd, arg);
+
+ if (ret != -ENOTTY)
+ return ret;
+
+ /* if we are in valid PWM mode, try it as a PWM ioctl as well */
+ switch (_mode) {
+ case MODE_2PWM:
+ case MODE_4PWM:
+ ret = pwm_ioctl(filp, cmd, arg);
+ break;
+
+ default:
+ debug("not in a PWM mode");
+ break;
+ }
+
+ /* if nobody wants it, let CDev have it */
+ if (ret == -ENOTTY)
+ ret = CDev::ioctl(filp, cmd, arg);
+
+ return ret;
+}
+
+int
+MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
+{
+ int ret = OK;
+ int channel;
+
+ lock();
+
+ switch (cmd) {
+ case PWM_SERVO_ARM:
+ ////up_pwm_servo_arm(true);
+ mk_servo_arm(true);
+ break;
+
+ case PWM_SERVO_DISARM:
+ ////up_pwm_servo_arm(false);
+ mk_servo_arm(false);
+ break;
+
+ case PWM_SERVO_SET_UPDATE_RATE:
+ set_pwm_rate(arg);
+ break;
+
+
+ case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
+
+ /* fake an update to the selected 'servo' channel */
+ if ((arg >= 0) && (arg <= 255)) {
+ channel = cmd - PWM_SERVO_SET(0);
+ //mk_servo_set(channel, arg);
+ } else {
+ ret = -EINVAL;
+ }
+
+ break;
+
+ case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
+ /* copy the current output value from the channel */
+ *(servo_position_t *)arg = cmd - PWM_SERVO_GET(0);
+ break;
+
+ case MIXERIOCGETOUTPUTCOUNT:
+ /*
+ if (_mode == MODE_4PWM) {
+ *(unsigned *)arg = 4;
+ } else {
+ *(unsigned *)arg = 2;
+ }
+ */
+
+ *(unsigned *)arg = _num_outputs;
+
+ break;
+
+ case MIXERIOCRESET:
+ if (_mixers != nullptr) {
+ delete _mixers;
+ _mixers = nullptr;
+ }
+
+ break;
+
+ case MIXERIOCADDSIMPLE: {
+ mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
+
+ SimpleMixer *mixer = new SimpleMixer(control_callback,
+ (uintptr_t)&_controls, mixinfo);
+
+ if (mixer->check()) {
+ delete mixer;
+ ret = -EINVAL;
+
+ } else {
+ if (_mixers == nullptr)
+ _mixers = new MixerGroup(control_callback,
+ (uintptr_t)&_controls);
+
+ _mixers->add_mixer(mixer);
+ }
+
+ break;
+ }
+
+ case MIXERIOCLOADBUF: {
+ const char *buf = (const char *)arg;
+ unsigned buflen = strnlen(buf, 1024);
+
+ if (_mixers == nullptr)
+ _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+
+ if (_mixers == nullptr) {
+ ret = -ENOMEM;
+
+ } else {
+
+ ret = _mixers->load_from_buf(buf, buflen);
+
+ if (ret != 0) {
+ debug("mixer load failed with %d", ret);
+ delete _mixers;
+ _mixers = nullptr;
+ ret = -EINVAL;
+ }
+ }
+ break;
+ }
+
+ default:
+ ret = -ENOTTY;
+ break;
+ }
+
+ unlock();
+
+ return ret;
+}
+
+void
+MK::gpio_reset(void)
+{
+ /*
+ * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
+ * to input mode.
+ */
+ for (unsigned i = 0; i < _ngpio; i++)
+ stm32_configgpio(_gpio_tab[i].input);
+
+ stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+ stm32_configgpio(GPIO_GPIO_DIR);
+}
+
+void
+MK::gpio_set_function(uint32_t gpios, int function)
+{
+ /*
+ * GPIOs 0 and 1 must have the same direction as they are buffered
+ * by a shared 2-port driver. Any attempt to set either sets both.
+ */
+ if (gpios & 3) {
+ gpios |= 3;
+
+ /* flip the buffer to output mode if required */
+ if (GPIO_SET_OUTPUT == function)
+ stm32_gpiowrite(GPIO_GPIO_DIR, 1);
+ }
+
+ /* configure selected GPIOs as required */
+ for (unsigned i = 0; i < _ngpio; i++) {
+ if (gpios & (1 << i)) {
+ switch (function) {
+ case GPIO_SET_INPUT:
+ stm32_configgpio(_gpio_tab[i].input);
+ break;
+
+ case GPIO_SET_OUTPUT:
+ stm32_configgpio(_gpio_tab[i].output);
+ break;
+
+ case GPIO_SET_ALT_1:
+ if (_gpio_tab[i].alt != 0)
+ stm32_configgpio(_gpio_tab[i].alt);
+
+ break;
+ }
+ }
+ }
+
+ /* flip buffer to input mode if required */
+ if ((GPIO_SET_INPUT == function) && (gpios & 3))
+ stm32_gpiowrite(GPIO_GPIO_DIR, 0);
+}
+
+void
+MK::gpio_write(uint32_t gpios, int function)
+{
+ int value = (function == GPIO_SET) ? 1 : 0;
+
+ for (unsigned i = 0; i < _ngpio; i++)
+ if (gpios & (1 << i))
+ stm32_gpiowrite(_gpio_tab[i].output, value);
+}
+
+uint32_t
+MK::gpio_read(void)
+{
+ uint32_t bits = 0;
+
+ for (unsigned i = 0; i < _ngpio; i++)
+ if (stm32_gpioread(_gpio_tab[i].input))
+ bits |= (1 << i);
+
+ return bits;
+}
+
+int
+MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ int ret = OK;
+
+ lock();
+
+ switch (cmd) {
+
+ case GPIO_RESET:
+ gpio_reset();
+ break;
+
+ case GPIO_SET_OUTPUT:
+ case GPIO_SET_INPUT:
+ case GPIO_SET_ALT_1:
+ gpio_set_function(arg, cmd);
+ break;
+
+ case GPIO_SET_ALT_2:
+ case GPIO_SET_ALT_3:
+ case GPIO_SET_ALT_4:
+ ret = -EINVAL;
+ break;
+
+ case GPIO_SET:
+ case GPIO_CLEAR:
+ gpio_write(arg, cmd);
+ break;
+
+ case GPIO_GET:
+ *(uint32_t *)arg = gpio_read();
+ break;
+
+ default:
+ ret = -ENOTTY;
+ }
+
+ unlock();
+
+ return ret;
+}
+
+namespace
+{
+
+enum PortMode {
+ PORT_MODE_UNSET = 0,
+ PORT_FULL_GPIO,
+ PORT_FULL_SERIAL,
+ PORT_FULL_PWM,
+ PORT_GPIO_AND_SERIAL,
+ PORT_PWM_AND_SERIAL,
+ PORT_PWM_AND_GPIO,
+};
+
+enum MappingMode {
+ MAPPING_MK = 0,
+ MAPPING_PX4,
+};
+
+ enum FrameType {
+ FRAME_PLUS = 0,
+ FRAME_X,
+ };
+
+PortMode g_port_mode;
+
+int
+mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype)
+{
+ uint32_t gpio_bits;
+ int shouldStop = 0;
+ MK::Mode servo_mode;
+
+ /* reset to all-inputs */
+ g_mk->ioctl(0, GPIO_RESET, 0);
+
+ gpio_bits = 0;
+ servo_mode = MK::MODE_NONE;
+
+ switch (new_mode) {
+ case PORT_FULL_GPIO:
+ case PORT_MODE_UNSET:
+ /* nothing more to do here */
+ break;
+
+ case PORT_FULL_SERIAL:
+ /* set all multi-GPIOs to serial mode */
+ gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
+ break;
+
+ case PORT_FULL_PWM:
+ /* select 4-pin PWM mode */
+ servo_mode = MK::MODE_4PWM;
+ break;
+
+ case PORT_GPIO_AND_SERIAL:
+ /* set RX/TX multi-GPIOs to serial mode */
+ gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+ break;
+
+ case PORT_PWM_AND_SERIAL:
+ /* select 2-pin PWM mode */
+ servo_mode = MK::MODE_2PWM;
+ /* set RX/TX multi-GPIOs to serial mode */
+ gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4;
+ break;
+
+ case PORT_PWM_AND_GPIO:
+ /* select 2-pin PWM mode */
+ servo_mode = MK::MODE_2PWM;
+ break;
+ }
+
+ /* adjust GPIO config for serial mode(s) */
+ if (gpio_bits != 0)
+ g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits);
+
+ /* native PX4 addressing) */
+ g_mk->set_px4mode(px4mode);
+
+ /* set frametype (geometry) */
+ g_mk->set_frametype(frametype);
+
+ /* motortest if enabled */
+ g_mk->set_motor_test(motortest);
+
+
+ /* (re)set count of used motors */
+ ////g_mk->set_motor_count(motorcount);
+ /* count used motors */
+
+ do {
+ if(g_mk->mk_check_for_blctrl(8, false) != 0) {
+ shouldStop = 4;
+ } else {
+ shouldStop++;
+ }
+ sleep(1);
+ } while ( shouldStop < 3);
+
+ g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
+
+ /* (re)set the PWM output mode */
+ g_mk->set_mode(servo_mode);
+
+
+ if ((servo_mode != MK::MODE_NONE) && (update_rate != 0))
+ g_mk->set_pwm_rate(update_rate);
+
+ return OK;
+}
+
+int
+mk_start(unsigned bus, unsigned motors)
+{
+ int ret = OK;
+
+ if (g_mk == nullptr) {
+
+ g_mk = new MK(bus);
+
+ if (g_mk == nullptr) {
+ ret = -ENOMEM;
+
+ } else {
+ ret = g_mk->init(motors);
+
+ if (ret != OK) {
+ delete g_mk;
+ g_mk = nullptr;
+ }
+ }
+ }
+
+ return ret;
+}
+
+
+} // namespace
+
+extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
+
+int
+mkblctrl_main(int argc, char *argv[])
+{
+ PortMode port_mode = PORT_FULL_PWM;
+ int pwm_update_rate_in_hz = UPDATE_RATE;
+ int motorcount = 8;
+ int bus = 1;
+ int px4mode = MAPPING_PX4;
+ int frametype = FRAME_PLUS; // + plus is default
+ bool motortest = false;
+ bool showHelp = false;
+ bool newMode = false;
+
+ /*
+ * optional parameters
+ */
+ for (int i = 1; i < argc; i++) {
+
+ /* look for the optional i2c bus parameter */
+ if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
+ if (argc > i + 1) {
+ bus = atoi(argv[i + 1]);
+ newMode = true;
+ } else {
+ errx(1, "missing argument for i2c bus (-b)");
+ return 1;
+ }
+ }
+
+ /* look for the optional frame parameter */
+ if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
+ if (argc > i + 1) {
+ if(strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
+ px4mode = MAPPING_MK;
+ newMode = true;
+ if(strcmp(argv[i + 1], "+") == 0) {
+ frametype = FRAME_PLUS;
+ } else {
+ frametype = FRAME_X;
+ }
+ } else {
+ errx(1, "only + or x for frametype supported !");
+ }
+ } else {
+ errx(1, "missing argument for mkmode (-mkmode)");
+ return 1;
+ }
+ }
+
+ /* look for the optional test parameter */
+ if (strcmp(argv[i], "-t") == 0) {
+ motortest = true;
+ newMode = true;
+ }
+
+ /* look for the optional -h --help parameter */
+ if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
+ showHelp == true;
+ }
+
+ }
+
+ if(showHelp) {
+ fprintf(stderr, "mkblctrl: help:\n");
+ fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
+ exit(1);
+ }
+
+
+ if (g_mk == nullptr) {
+ if (mk_start(bus, motorcount) != OK) {
+ errx(1, "failed to start the MK-BLCtrl driver");
+ } else {
+ newMode = true;
+ }
+ }
+
+
+ /* parameter set ? */
+ if (newMode) {
+ /* switch parameter */
+ return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype);
+ }
+
+ /* test, etc. here g*/
+
+ exit(1);
+}
diff --git a/apps/commander/Makefile b/src/drivers/mkblctrl/module.mk
index d12697274..3ac263b00 100644
--- a/apps/commander/Makefile
+++ b/src/drivers/mkblctrl/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012,2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,14 +32,11 @@
############################################################################
#
-# Commander application
+# Interface driver for the Mikrokopter BLCtrl
#
-APPNAME = commander
-PRIORITY = SCHED_PRIORITY_MAX - 30
-STACKSIZE = 2048
+MODULE_COMMAND = mkblctrl
-INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
-
-include $(APPDIR)/mk/app.mk
+SRCS = mkblctrl.cpp
+INCLUDE_DIRS += $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
diff --git a/apps/mavlink/Makefile b/src/drivers/mpu6000/module.mk
index 8ddb69b71..c7d9cd3ef 100644
--- a/apps/mavlink/Makefile
+++ b/src/drivers/mpu6000/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,13 +32,12 @@
############################################################################
#
-# Mavlink Application
+# Makefile to build the MPU6000 driver.
#
-APPNAME = mavlink
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+MODULE_COMMAND = mpu6000
-INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
+# XXX seems excessive, check if 2048 is not sufficient
+MODULE_STACKSIZE = 4096
-include $(APPDIR)/mk/app.mk
+SRCS = mpu6000.cpp
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index ce7062046..df1958186 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
diff --git a/apps/drivers/led/Makefile b/src/drivers/ms5611/module.mk
index 7de188259..3c4b0f093 100644
--- a/apps/drivers/led/Makefile
+++ b/src/drivers/ms5611/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,7 +32,9 @@
############################################################################
#
-# Makefile to build the LED driver.
+# MS5611 driver
#
-include $(APPDIR)/mk/app.mk
+MODULE_COMMAND = ms5611
+
+SRCS = ms5611.cpp
diff --git a/apps/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 59ab5936e..59ab5936e 100644
--- a/apps/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
diff --git a/apps/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index e54724536..bf72892eb 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -505,7 +505,11 @@ PX4FMU::task_main()
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
/* update PWM servo armed status if armed and not locked down */
- up_pwm_servo_arm(aa.armed && !aa.lockdown);
+ bool set_armed = aa.armed && !aa.lockdown;
+ if (set_armed != _armed) {
+ _armed = set_armed;
+ up_pwm_servo_arm(set_armed);
+ }
}
// see if we have new PPM input data
@@ -602,6 +606,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
up_pwm_servo_arm(true);
break;
+ case PWM_SERVO_SET_ARM_OK:
+ case PWM_SERVO_CLEAR_ARM_OK:
+ // these are no-ops, as no safety switch
+ break;
+
case PWM_SERVO_DISARM:
up_pwm_servo_arm(false);
break;
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
new file mode 100644
index 000000000..05bc7a5b3
--- /dev/null
+++ b/src/drivers/px4fmu/module.mk
@@ -0,0 +1,6 @@
+#
+# Interface driver for the PX4FMU board
+#
+
+MODULE_COMMAND = fmu
+SRCS = fmu.cpp
diff --git a/apps/drivers/px4io/Makefile b/src/drivers/px4io/module.mk
index cbd942546..328e5a684 100644
--- a/apps/drivers/px4io/Makefile
+++ b/src/drivers/px4io/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,8 +35,7 @@
# Interface driver for the PX4IO board.
#
-APPNAME = px4io
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+MODULE_COMMAND = px4io
-include $(APPDIR)/mk/app.mk
+SRCS = px4io.cpp \
+ uploader.cpp
diff --git a/apps/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index f72a2c6b5..399c003b7 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -80,11 +80,11 @@
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/parameter_update.h>
+#include <debug.h>
-#include <px4io/protocol.h>
#include <mavlink/mavlink_log.h>
#include "uploader.h"
-#include <debug.h>
+#include <modules/px4iofirmware/protocol.h>
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
@@ -109,6 +109,14 @@ public:
int set_update_rate(int rate);
/**
+ * Set the battery current scaling and bias
+ *
+ * @param amp_per_volt
+ * @param amp_bias
+ */
+ void set_battery_current_scaling(float amp_per_volt, float amp_bias);
+
+ /**
* Print the current status of IO
*/
void print_status();
@@ -151,6 +159,10 @@ private:
bool _primary_pwm_device; ///< true if we are the default PWM output
+ float _battery_amp_per_volt;
+ float _battery_amp_bias;
+ float _battery_mamphour_total;
+ uint64_t _battery_last_timestamp;
/**
* Trampoline to the worker task
@@ -314,6 +326,10 @@ PX4IO::PX4IO() :
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
+ _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
+ _battery_amp_bias(0),
+ _battery_mamphour_total(0),
+ _battery_last_timestamp(0),
_primary_pwm_device(false)
{
/* we need this potentially before it could be set in task_main */
@@ -400,7 +416,7 @@ PX4IO::init()
* already armed, and has been configured for in-air restart
*/
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
- (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) {
+ (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
@@ -484,10 +500,9 @@ PX4IO::init()
/* dis-arm IO before touching anything */
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
- PX4IO_P_SETUP_ARMING_ARM_OK |
+ PX4IO_P_SETUP_ARMING_FMU_ARMED |
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
- PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
- PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
+ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
/* publish RC config to IO */
ret = io_set_rc_config();
@@ -686,17 +701,18 @@ PX4IO::io_set_arming_state()
uint16_t set = 0;
uint16_t clear = 0;
- if (armed.armed) {
- set |= PX4IO_P_SETUP_ARMING_ARM_OK;
+ if (armed.armed && !armed.lockdown) {
+ set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
- clear |= PX4IO_P_SETUP_ARMING_ARM_OK;
+ clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
// TODO fix this
- // if (vstatus.flag_vector_flight_mode_ok) {
- // set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
- // } else {
- // clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
- // }
+// if (armed.ready_to_arm) {
+// set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
+// } else {
+// clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
+// }
+
if (vstatus.flag_external_manual_override_ok) {
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else {
@@ -885,11 +901,22 @@ PX4IO::io_get_status()
/* voltage is scaled to mV */
battery_status.voltage_v = regs[2] / 1000.0f;
- /* current scaling should be to cA in order to avoid limiting at 65A */
- battery_status.current_a = regs[3] / 100.f;
+ /*
+ regs[3] contains the raw ADC count, as 12 bit ADC
+ value, with full range being 3.3v
+ */
+ battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt;
+ battery_status.current_a += _battery_amp_bias;
- /* this requires integration over time - not currently implemented */
- battery_status.discharged_mah = -1.0f;
+ /*
+ integrate battery over time to get total mAh used
+ */
+ if (_battery_last_timestamp != 0) {
+ _battery_mamphour_total += battery_status.current_a *
+ (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
+ }
+ battery_status.discharged_mah = _battery_mamphour_total;
+ _battery_last_timestamp = battery_status.timestamp;
/* lazily publish the battery voltage */
if (_to_battery > 0) {
@@ -1246,9 +1273,14 @@ PX4IO::print_status()
((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""));
- printf("vbatt %u ibatt %u\n",
- io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
- io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT));
+ printf("vbatt %u ibatt %u vbatt scale %u\n",
+ io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
+ io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
+ io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
+ printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
+ (double)_battery_amp_per_volt,
+ (double)_battery_amp_bias,
+ (double)_battery_mamphour_total);
printf("actuators");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
@@ -1280,19 +1312,15 @@ PX4IO::print_status()
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s\n",
arming,
- ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
- ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
- printf("vbatt scale %u ibatt scale %u ibatt bias %u\n",
- io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE),
- io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE),
- io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS));
printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
printf("controls");
for (unsigned i = 0; i < _max_controls; i++)
@@ -1327,21 +1355,27 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
switch (cmd) {
case PWM_SERVO_ARM:
/* set the 'armed' bit */
- ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK);
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED);
+ break;
+
+ case PWM_SERVO_SET_ARM_OK:
+ /* set the 'OK to arm' bit */
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK);
+ break;
+
+ case PWM_SERVO_CLEAR_ARM_OK:
+ /* clear the 'OK to arm' bit */
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0);
break;
case PWM_SERVO_DISARM:
/* clear the 'armed' bit */
- ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
break;
case PWM_SERVO_SET_UPDATE_RATE:
/* set the requested alternate rate */
- if ((arg >= 50) && (arg <= 400)) { /* TODO: we could go higher for e.g. TurboPWM */
- ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
- } else {
- ret = -EINVAL;
- }
+ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
break;
case PWM_SERVO_SELECT_UPDATE_RATE: {
@@ -1526,6 +1560,12 @@ PX4IO::set_update_rate(int rate)
return 0;
}
+void
+PX4IO::set_battery_current_scaling(float amp_per_volt, float amp_bias)
+{
+ _battery_amp_per_volt = amp_per_volt;
+ _battery_amp_bias = amp_bias;
+}
extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
@@ -1663,6 +1703,18 @@ px4io_main(int argc, char *argv[])
exit(0);
}
+ if (!strcmp(argv[1], "current")) {
+ if (g_dev != nullptr) {
+ if ((argc > 3)) {
+ g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
+ } else {
+ errx(1, "missing argument (apm_per_volt, amp_offset)");
+ return 1;
+ }
+ }
+ exit(0);
+ }
+
if (!strcmp(argv[1], "recovery")) {
if (g_dev != nullptr) {
@@ -1790,5 +1842,5 @@ px4io_main(int argc, char *argv[])
monitor();
out:
- errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit' or 'update'");
+ errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'");
}
diff --git a/apps/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp
index abf59216a..15524c3ae 100644
--- a/apps/drivers/px4io/uploader.cpp
+++ b/src/drivers/px4io/uploader.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,7 +32,8 @@
****************************************************************************/
/**
- * @file Firmware uploader for PX4IO
+ * @file uploader.cpp
+ * Firmware uploader for PX4IO
*/
#include <nuttx/config.h>
@@ -126,6 +127,8 @@ PX4IO_Uploader::upload(const char *filenames[])
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
+ close(_io_fd);
+ _io_fd = -1;
return -EIO;
}
@@ -144,18 +147,25 @@ PX4IO_Uploader::upload(const char *filenames[])
if (filename == NULL) {
log("no firmware found");
+ close(_io_fd);
+ _io_fd = -1;
return -ENOENT;
}
struct stat st;
if (stat(filename, &st) != 0) {
log("Failed to stat %s - %d\n", filename, (int)errno);
+ close(_io_fd);
+ _io_fd = -1;
return -errno;
}
fw_size = st.st_size;
- if (_fw_fd == -1)
+ if (_fw_fd == -1) {
+ close(_io_fd);
+ _io_fd = -1;
return -ENOENT;
+ }
/* do the usual program thing - allow for failure */
for (unsigned retries = 0; retries < 1; retries++) {
@@ -166,6 +176,8 @@ PX4IO_Uploader::upload(const char *filenames[])
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
+ close(_io_fd);
+ _io_fd = -1;
return -EIO;
}
}
@@ -177,6 +189,8 @@ PX4IO_Uploader::upload(const char *filenames[])
log("found bootloader revision: %d", bl_rev);
} else {
log("found unsupported bootloader revision %d, exiting", bl_rev);
+ close(_io_fd);
+ _io_fd = -1;
return OK;
}
}
@@ -220,6 +234,8 @@ PX4IO_Uploader::upload(const char *filenames[])
}
close(_fw_fd);
+ close(_io_fd);
+ _io_fd = -1;
return ret;
}
diff --git a/apps/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h
index f983d1981..135202dd1 100644
--- a/apps/drivers/px4io/uploader.h
+++ b/src/drivers/px4io/uploader.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,7 +32,8 @@
****************************************************************************/
/**
- * @file Firmware uploader for PX4IO
+ * @file uploader.h
+ * Firmware uploader definitions for PX4IO.
*/
#ifndef _PX4IO_UPLOADER_H
diff --git a/apps/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 911def943..911def943 100644
--- a/apps/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk
new file mode 100644
index 000000000..48620feea
--- /dev/null
+++ b/src/drivers/stm32/adc/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# STM32 ADC driver
+#
+
+MODULE_COMMAND = adc
+
+SRCS = adc.cpp
+
+INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/apps/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index bb67d5e6d..cec0c49ff 100644
--- a/apps/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -125,7 +125,7 @@
# define HRT_TIMER_VECTOR STM32_IRQ_TIM8CC
# define HRT_TIMER_CLOCK STM32_APB2_TIM8_CLKIN
# if CONFIG_STM32_TIM8
-# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=6
+# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=8
# endif
#elif HRT_TIMER == 9
# define HRT_TIMER_BASE STM32_TIM9_BASE
diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c
index d7316e1f7..c1efb8515 100644
--- a/apps/drivers/stm32/drv_pwm_servo.c
+++ b/src/drivers/stm32/drv_pwm_servo.c
@@ -290,6 +290,8 @@ up_pwm_servo_set_rate(unsigned rate)
{
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++)
up_pwm_servo_set_rate_group_update(i, rate);
+
+ return 0;
}
uint32_t
diff --git a/apps/drivers/stm32/drv_pwm_servo.h b/src/drivers/stm32/drv_pwm_servo.h
index 5dd4cf70c..5dd4cf70c 100644
--- a/apps/drivers/stm32/drv_pwm_servo.h
+++ b/src/drivers/stm32/drv_pwm_servo.h
diff --git a/apps/drivers/stm32/Makefile b/src/drivers/stm32/module.mk
index 4ad57f413..bb751c7f6 100644
--- a/apps/drivers/stm32/Makefile
+++ b/src/drivers/stm32/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -37,6 +37,7 @@
# Modules in this directory are compiled for all STM32 targets.
#
-INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+SRCS = drv_hrt.c \
+ drv_pwm_servo.c
-include $(APPDIR)/mk/app.mk
+INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk
new file mode 100644
index 000000000..827cf30b2
--- /dev/null
+++ b/src/drivers/stm32/tone_alarm/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Tone alarm driver
+#
+
+MODULE_COMMAND = tone_alarm
+
+SRCS = tone_alarm.cpp
+
+INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index baa652d8a..ac5511e60 100644
--- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -494,7 +494,7 @@ ToneAlarm::init()
return ret;
/* configure the GPIO to the idle state */
- stm32_configgpio(GPIO_TONE_ALARM);
+ stm32_configgpio(GPIO_TONE_ALARM_IDLE);
/* clock/power on our timer */
modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
@@ -606,6 +606,8 @@ ToneAlarm::start_note(unsigned note)
rEGR = GTIM_EGR_UG; // force a reload of the period
rCCER |= TONE_CCER; // enable the output
+ // configure the GPIO to enable timer output
+ stm32_configgpio(GPIO_TONE_ALARM);
}
void
@@ -616,10 +618,8 @@ ToneAlarm::stop_note()
/*
* Make sure the GPIO is not driving the speaker.
- *
- * XXX this presumes PX4FMU and the onboard speaker driver FET.
*/
- stm32_gpiowrite(GPIO_TONE_ALARM, 0);
+ stm32_configgpio(GPIO_TONE_ALARM_IDLE);
}
void
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
new file mode 100644
index 000000000..7bf99785c
--- /dev/null
+++ b/src/examples/fixedwing_control/main.c
@@ -0,0 +1,477 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ * Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file main.c
+ * Implementation of a fixed wing attitude controller. This file is a complete
+ * fixed wing controller flying manual attitude control or auto waypoint control.
+ * There is no need to touch any other system components to extend / modify the
+ * complete control architecture.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/pid/pid.h>
+#include <systemlib/geo/geo.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+
+/* process-specific header files */
+#include "params.h"
+
+/* Prototypes */
+/**
+ * Daemon management function.
+ */
+__EXPORT int ex_fixedwing_control_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of daemon.
+ */
+int fixedwing_control_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
+ float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp,
+ struct actuator_controls_s *actuators);
+
+void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
+ const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
+
+/* Variables */
+static bool thread_should_exit = false; /**< Daemon exit flag */
+static bool thread_running = false; /**< Daemon status flag */
+static int deamon_task; /**< Handle of deamon task / thread */
+static struct params p;
+static struct param_handles ph;
+
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
+ float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp,
+ struct actuator_controls_s *actuators)
+{
+
+ /*
+ * The PX4 architecture provides a mixer outside of the controller.
+ * The mixer is fed with a default vector of actuator controls, representing
+ * moments applied to the vehicle frame. This vector
+ * is structured as:
+ *
+ * Control Group 0 (attitude):
+ *
+ * 0 - roll (-1..+1)
+ * 1 - pitch (-1..+1)
+ * 2 - yaw (-1..+1)
+ * 3 - thrust ( 0..+1)
+ * 4 - flaps (-1..+1)
+ * ...
+ *
+ * Control Group 1 (payloads / special):
+ *
+ * ...
+ */
+
+ /*
+ * Calculate roll error and apply P gain
+ */
+ float roll_err = att->roll - att_sp->roll_body;
+ actuators->control[0] = roll_err * p.roll_p;
+
+ /*
+ * Calculate pitch error and apply P gain
+ */
+ float pitch_err = att->pitch - att_sp->pitch_body;
+ actuators->control[1] = pitch_err * p.pitch_p;
+}
+
+void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
+ const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
+{
+
+ /*
+ * Calculate heading error of current position to desired position
+ */
+
+ /* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution */
+ float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
+
+ /* calculate heading error */
+ float yaw_err = att->yaw - bearing;
+ /* apply control gain */
+ att_sp->roll_body = yaw_err * p.hdng_p;
+}
+
+/* Main Thread */
+int fixedwing_control_thread_main(int argc, char *argv[])
+{
+ /* read arguments */
+ bool verbose = false;
+
+ for (int i = 1; i < argc; i++) {
+ if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
+ verbose = true;
+ }
+ }
+
+ /* welcome user (warnx prints a line, including an appended\n, with variable arguments */
+ warnx("[example fixedwing control] started");
+
+ /* initialize parameters, first the handles, then the values */
+ parameters_init(&ph);
+ parameters_update(&ph, &p);
+
+ /* declare and safely initialize all structs to zero */
+ struct vehicle_attitude_s att;
+ memset(&att, 0, sizeof(att));
+ struct vehicle_attitude_setpoint_s att_sp;
+ memset(&att_sp, 0, sizeof(att_sp));
+ struct vehicle_rates_setpoint_s rates_sp;
+ memset(&rates_sp, 0, sizeof(rates_sp));
+ struct vehicle_global_position_s global_pos;
+ memset(&global_pos, 0, sizeof(global_pos));
+ struct manual_control_setpoint_s manual_sp;
+ memset(&manual_sp, 0, sizeof(manual_sp));
+ struct vehicle_status_s vstatus;
+ memset(&vstatus, 0, sizeof(vstatus));
+ struct vehicle_global_position_setpoint_s global_sp;
+ memset(&global_sp, 0, sizeof(global_sp));
+
+ /* output structs */
+ struct actuator_controls_s actuators;
+ memset(&actuators, 0, sizeof(actuators));
+
+
+ /* publish actuator controls */
+ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
+ actuators.control[i] = 0.0f;
+ }
+
+ orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+ orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+
+ /* subscribe */
+ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+ int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+ int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+ int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+ int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+ int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+ int param_sub = orb_subscribe(ORB_ID(parameter_update));
+
+ /* Setup of loop */
+ float gyro[3] = {0.0f, 0.0f, 0.0f};
+ float speed_body[3] = {0.0f, 0.0f, 0.0f};
+ struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
+ { .fd = att_sub, .events = POLLIN }};
+
+ while (!thread_should_exit) {
+
+ /*
+ * Wait for a sensor or param update, check for exit condition every 500 ms.
+ * This means that the execution will block here without consuming any resources,
+ * but will continue to execute the very moment a new attitude measurement or
+ * a param update is published. So no latency in contrast to the polling
+ * design pattern (do not confuse the poll() system call with polling).
+ *
+ * This design pattern makes the controller also agnostic of the attitude
+ * update speed - it runs as fast as the attitude updates with minimal latency.
+ */
+ int ret = poll(fds, 2, 500);
+
+ if (ret < 0) {
+ /* poll error, this will not really happen in practice */
+ warnx("poll error");
+
+ } else if (ret == 0) {
+ /* no return value = nothing changed for 500 ms, ignore */
+ } else {
+
+ /* only update parameters if they changed */
+ if (fds[0].revents & POLLIN) {
+ /* read from param to clear updated flag (uORB API requirement) */
+ struct parameter_update_s update;
+ orb_copy(ORB_ID(parameter_update), param_sub, &update);
+
+ /* if a param update occured, re-read our parameters */
+ parameters_update(&ph, &p);
+ }
+
+ /* only run controller if attitude changed */
+ if (fds[1].revents & POLLIN) {
+
+
+ /* Check if there is a new position measurement or position setpoint */
+ bool pos_updated;
+ orb_check(global_pos_sub, &pos_updated);
+ bool global_sp_updated;
+ orb_check(global_sp_sub, &global_sp_updated);
+
+ /* get a local copy of attitude */
+ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+
+ if (global_sp_updated)
+ orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
+
+ if (pos_updated) {
+ orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+
+ if (att.R_valid) {
+ speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
+ speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
+ speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
+
+ } else {
+ speed_body[0] = 0;
+ speed_body[1] = 0;
+ speed_body[2] = 0;
+
+ warnx("Did not get a valid R\n");
+ }
+ }
+
+ orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
+ orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
+
+ gyro[0] = att.rollspeed;
+ gyro[1] = att.pitchspeed;
+ gyro[2] = att.yawspeed;
+
+ /* control */
+
+#warning fix this
+#if 0
+ if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ ||
+ vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) {
+
+ /* simple heading control */
+ control_heading(&global_pos, &global_sp, &att, &att_sp);
+
+ /* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */
+ actuators.control[1] = 0.0f;
+ actuators.control[2] = 0.0f;
+
+ /* simple attitude control */
+ control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
+
+ /* pass through throttle */
+ actuators.control[3] = att_sp.thrust;
+
+ /* set flaps to zero */
+ actuators.control[4] = 0.0f;
+
+ } else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) {
+ if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+
+ /* if the RC signal is lost, try to stay level and go slowly back down to ground */
+ if (vstatus.rc_signal_lost) {
+
+ /* put plane into loiter */
+ att_sp.roll_body = 0.3f;
+ att_sp.pitch_body = 0.0f;
+
+ /* limit throttle to 60 % of last value if sane */
+ if (isfinite(manual_sp.throttle) &&
+ (manual_sp.throttle >= 0.0f) &&
+ (manual_sp.throttle <= 1.0f)) {
+ att_sp.thrust = 0.6f * manual_sp.throttle;
+
+ } else {
+ att_sp.thrust = 0.0f;
+ }
+
+ att_sp.yaw_body = 0;
+
+ // XXX disable yaw control, loiter
+
+ } else {
+
+ att_sp.roll_body = manual_sp.roll;
+ att_sp.pitch_body = manual_sp.pitch;
+ att_sp.yaw_body = 0;
+ att_sp.thrust = manual_sp.throttle;
+ }
+
+ att_sp.timestamp = hrt_absolute_time();
+
+ /* attitude control */
+ control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
+
+ /* pass through throttle */
+ actuators.control[3] = att_sp.thrust;
+
+ /* pass through flaps */
+ if (isfinite(manual_sp.flaps)) {
+ actuators.control[4] = manual_sp.flaps;
+
+ } else {
+ actuators.control[4] = 0.0f;
+ }
+
+ } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+ /* directly pass through values */
+ actuators.control[0] = manual_sp.roll;
+ /* positive pitch means negative actuator -> pull up */
+ actuators.control[1] = manual_sp.pitch;
+ actuators.control[2] = manual_sp.yaw;
+ actuators.control[3] = manual_sp.throttle;
+
+ if (isfinite(manual_sp.flaps)) {
+ actuators.control[4] = manual_sp.flaps;
+
+ } else {
+ actuators.control[4] = 0.0f;
+ }
+ }
+ }
+#endif
+
+ /* publish rates */
+ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
+
+ /* sanity check and publish actuator outputs */
+ if (isfinite(actuators.control[0]) &&
+ isfinite(actuators.control[1]) &&
+ isfinite(actuators.control[2]) &&
+ isfinite(actuators.control[3])) {
+ orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+ }
+ }
+ }
+ }
+
+ printf("[ex_fixedwing_control] exiting, stopping all motors.\n");
+ thread_running = false;
+
+ /* kill all outputs */
+ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
+ actuators.control[i] = 0.0f;
+
+ orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+
+ fflush(stdout);
+
+ return 0;
+}
+
+/* Startup Functions */
+
+static void
+usage(const char *reason)
+{
+ if (reason)
+ fprintf(stderr, "%s\n", reason);
+
+ fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n");
+ exit(1);
+}
+
+/**
+ * The daemon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int ex_fixedwing_control_main(int argc, char *argv[])
+{
+ if (argc < 1)
+ usage("missing command");
+
+ if (!strcmp(argv[1], "start")) {
+
+ if (thread_running) {
+ printf("ex_fixedwing_control already running\n");
+ /* this is not an error */
+ exit(0);
+ }
+
+ thread_should_exit = false;
+ deamon_task = task_spawn("ex_fixedwing_control",
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 20,
+ 2048,
+ fixedwing_control_thread_main,
+ (argv) ? (const char **)&argv[2] : (const char **)NULL);
+ thread_running = true;
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "stop")) {
+ thread_should_exit = true;
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "status")) {
+ if (thread_running) {
+ printf("\tex_fixedwing_control is running\n");
+
+ } else {
+ printf("\tex_fixedwing_control not started\n");
+ }
+
+ exit(0);
+ }
+
+ usage("unrecognized command");
+ exit(1);
+}
+
+
+
diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk
new file mode 100644
index 000000000..d2c48934f
--- /dev/null
+++ b/src/examples/fixedwing_control/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Fixedwing Attitude Control Demo / Example Application
+#
+
+MODULE_COMMAND = ex_fixedwing_control
+
+SRCS = main.c \
+ params.c
diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c
new file mode 100644
index 000000000..8042c74f5
--- /dev/null
+++ b/src/examples/fixedwing_control/params.c
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ * Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file params.c
+ *
+ * Parameters for fixedwing demo
+ */
+
+#include "params.h"
+
+/* controller parameters, use max. 15 characters for param name! */
+
+/**
+ *
+ */
+PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.2f);
+
+/**
+ *
+ */
+PARAM_DEFINE_FLOAT(EXFW_ROLL_P, 0.2f);
+
+/**
+ *
+ */
+PARAM_DEFINE_FLOAT(EXFW_PITCH_P, 0.2f);
+
+int parameters_init(struct param_handles *h)
+{
+ /* PID parameters */
+ h->hdng_p = param_find("EXFW_HDNG_P");
+ h->roll_p = param_find("EXFW_ROLL_P");
+ h->pitch_p = param_find("EXFW_PITCH_P");
+
+ return OK;
+}
+
+int parameters_update(const struct param_handles *h, struct params *p)
+{
+ param_get(h->hdng_p, &(p->hdng_p));
+ param_get(h->roll_p, &(p->roll_p));
+ param_get(h->pitch_p, &(p->pitch_p));
+
+ return OK;
+}
diff --git a/src/examples/fixedwing_control/params.h b/src/examples/fixedwing_control/params.h
new file mode 100644
index 000000000..4ae8e90ec
--- /dev/null
+++ b/src/examples/fixedwing_control/params.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ * Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file params.h
+ *
+ * Definition of parameters for fixedwing example
+ */
+
+#include <systemlib/param/param.h>
+
+struct params {
+ float hdng_p;
+ float roll_p;
+ float pitch_p;
+};
+
+struct param_handles {
+ param_t hdng_p;
+ param_t roll_p;
+ param_t pitch_p;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct param_handles *h, struct params *p);
diff --git a/apps/examples/math_demo/math_demo.cpp b/src/examples/math_demo/math_demo.cpp
index a9c556748..36d3c2e84 100644
--- a/apps/examples/math_demo/math_demo.cpp
+++ b/src/examples/math_demo/math_demo.cpp
@@ -1,7 +1,7 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: @author Example User <mail@example.com>
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Author: James Goppert
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,6 +34,7 @@
/**
* @file math_demo.cpp
+ * @author James Goppert
* Demonstration of math library
*/
diff --git a/apps/drivers/ms5611/Makefile b/src/examples/math_demo/module.mk
index d8e67cba2..deba13fd0 100644
--- a/apps/drivers/ms5611/Makefile
+++ b/src/examples/math_demo/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,11 +32,10 @@
############################################################################
#
-# MS5611 driver
+# Mathlib / operations demo application
#
-APPNAME = ms5611
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+MODULE_COMMAND = math_demo
+MODULE_STACKSIZE = 12000
-include $(APPDIR)/mk/app.mk
+SRCS = math_demo.cpp
diff --git a/src/examples/px4_daemon_app/module.mk b/src/examples/px4_daemon_app/module.mk
new file mode 100644
index 000000000..5f8aa73d5
--- /dev/null
+++ b/src/examples/px4_daemon_app/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Daemon application
+#
+
+MODULE_COMMAND = px4_daemon_app
+
+SRCS = px4_daemon_app.c
diff --git a/apps/examples/px4_deamon_app/px4_deamon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c
index a5d847777..a5d847777 100644
--- a/apps/examples/px4_deamon_app/px4_deamon_app.c
+++ b/src/examples/px4_daemon_app/px4_daemon_app.c
diff --git a/apps/examples/math_demo/Makefile b/src/examples/px4_mavlink_debug/module.mk
index a1105899a..fefd61496 100644
--- a/apps/examples/math_demo/Makefile
+++ b/src/examples/px4_mavlink_debug/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,8 +35,6 @@
# Basic example application
#
-APPNAME = math_demo
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 8192
+MODULE_COMMAND = px4_mavlink_debug
-include $(APPDIR)/mk/app.mk
+SRCS = px4_mavlink_debug.c \ No newline at end of file
diff --git a/apps/examples/px4_mavlink_debug/px4_mavlink_debug.c b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c
index aa1eb5ed8..aa1eb5ed8 100644
--- a/apps/examples/px4_mavlink_debug/px4_mavlink_debug.c
+++ b/src/examples/px4_mavlink_debug/px4_mavlink_debug.c
diff --git a/src/examples/px4_simple_app/module.mk b/src/examples/px4_simple_app/module.mk
new file mode 100644
index 000000000..32b06c3a5
--- /dev/null
+++ b/src/examples/px4_simple_app/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Basic example application
+#
+
+MODULE_COMMAND = px4_simple_app
+
+SRCS = px4_simple_app.c
diff --git a/apps/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c
index 7f655964d..7f655964d 100644
--- a/apps/examples/px4_simple_app/px4_simple_app.c
+++ b/src/examples/px4_simple_app/px4_simple_app.c
diff --git a/apps/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h
index 233a76cb3..233a76cb3 100644
--- a/apps/mavlink/mavlink_log.h
+++ b/src/include/mavlink/mavlink_log.h
diff --git a/apps/systemlib/visibility.h b/src/include/visibility.h
index 2c6adc062..2c6adc062 100644
--- a/apps/systemlib/visibility.h
+++ b/src/include/visibility.h
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 6df454a55..4ef150da1 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -355,6 +355,8 @@ int KalmanNav::predictState(float dt)
float LDot = vN / R;
float lDot = vE / (cosLSing * R);
float rotRate = 2 * omega + lDot;
+
+ // XXX position prediction using speed
float vNDot = fN - vE * rotRate * sinL +
vD * LDot;
float vDDot = fD - vE * rotRate * cosL -
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index c2bf18115..c2bf18115 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
diff --git a/apps/examples/kalman_demo/kalman_demo.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
index 581b68b01..aebe3d1fe 100644
--- a/apps/examples/kalman_demo/kalman_demo.cpp
+++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
@@ -55,7 +55,7 @@ static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
-extern "C" __EXPORT int kalman_demo_main(int argc, char *argv[]);
+extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
@@ -85,7 +85,7 @@ usage(const char *reason)
* The actual stack size should be set in the call
* to task_create().
*/
-int kalman_demo_main(int argc, char *argv[])
+int att_pos_estimator_ekf_main(int argc, char *argv[])
{
if (argc < 1)
diff --git a/apps/drivers/px4fmu/Makefile b/src/modules/att_pos_estimator_ekf/module.mk
index 7f7c2a732..21b7c9166 100644
--- a/apps/drivers/px4fmu/Makefile
+++ b/src/modules/att_pos_estimator_ekf/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,13 +32,14 @@
############################################################################
#
-# Interface driver for the PX4FMU board
+# Full attitude / position Extended Kalman Filter
#
-APPNAME = fmu
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+MODULE_COMMAND = att_pos_estimator_ekf
-INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+# XXX this might be intended for the spawned deamon, validate
+MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
-include $(APPDIR)/mk/app.mk
+SRCS = kalman_main.cpp \
+ KalmanNav.cpp \
+ params.c
diff --git a/apps/examples/kalman_demo/params.c b/src/modules/att_pos_estimator_ekf/params.c
index 50642f067..50642f067 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/src/modules/att_pos_estimator_ekf/params.c
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index bd972f03f..8e18c3c9a 100755
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -46,7 +46,6 @@
#include <stdbool.h>
#include <poll.h>
#include <fcntl.h>
-#include <v1.0/common/mavlink.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
@@ -66,11 +65,17 @@
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
#include "codegen/attitudeKalmanfilter_initialize.h"
#include "codegen/attitudeKalmanfilter.h"
#include "attitude_estimator_ekf_params.h"
+#ifdef __cplusplus
+}
+#endif
-__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
+extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
@@ -265,10 +270,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* Main loop*/
while (!thread_should_exit) {
- struct pollfd fds[2] = {
- { .fd = sub_raw, .events = POLLIN },
- { .fd = sub_params, .events = POLLIN }
- };
+ struct pollfd fds[2];
+ fds[0].fd = sub_raw;
+ fds[0].events = POLLIN;
+ fds[1].fd = sub_params;
+ fds[1].events = POLLIN;
int ret = poll(fds, 2, 1000);
if (ret < 0) {
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index 7d3812abd..7d3812abd 100755
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
index 09817d58e..09817d58e 100755
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
index 9e97ad11a..9e97ad11a 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
index afa63c1a9..afa63c1a9 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
index 7d620d7fa..7d620d7fa 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
index 8b599be66..8b599be66 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
index 7f9727419..7f9727419 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
index da84a5024..da84a5024 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
index 30fd1e75e..30fd1e75e 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c
index 84ada9002..84ada9002 100755
--- a/apps/attitude_estimator_ekf/codegen/cross.c
+++ b/src/modules/attitude_estimator_ekf/codegen/cross.c
diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h
index e727f0684..e727f0684 100755
--- a/apps/attitude_estimator_ekf/codegen/cross.h
+++ b/src/modules/attitude_estimator_ekf/codegen/cross.h
diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c
index b89ab58ef..b89ab58ef 100755
--- a/apps/attitude_estimator_ekf/codegen/eye.c
+++ b/src/modules/attitude_estimator_ekf/codegen/eye.c
diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h
index 94fbe7671..94fbe7671 100755
--- a/apps/attitude_estimator_ekf/codegen/eye.h
+++ b/src/modules/attitude_estimator_ekf/codegen/eye.h
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
index a810f22e4..a810f22e4 100755
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.c
+++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
index 2d3b0d51f..2d3b0d51f 100755
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.h
+++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c
index 0c418cc7b..0c418cc7b 100755
--- a/apps/attitude_estimator_ekf/codegen/norm.c
+++ b/src/modules/attitude_estimator_ekf/codegen/norm.c
diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h
index 60cf77b57..60cf77b57 100755
--- a/apps/attitude_estimator_ekf/codegen/norm.h
+++ b/src/modules/attitude_estimator_ekf/codegen/norm.h
diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c
index d035dae5e..d035dae5e 100755
--- a/apps/attitude_estimator_ekf/codegen/rdivide.c
+++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.c
diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h
index 4bbebebe2..4bbebebe2 100755
--- a/apps/attitude_estimator_ekf/codegen/rdivide.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.h
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
index 34164d104..34164d104 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
index 145373cd0..145373cd0 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
index d84ca9573..d84ca9573 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
index 65fdaa96f..65fdaa96f 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
index 356498363..356498363 100755
--- a/apps/attitude_estimator_ekf/codegen/rt_defines.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
index 303d1d9d2..303d1d9d2 100755
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
+++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
index bd56b30d9..bd56b30d9 100755
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
index 9a5c96267..9a5c96267 100755
--- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
diff --git a/apps/attitude_estimator_ekf/Makefile b/src/modules/attitude_estimator_ekf/module.mk
index 734af7cc1..d98647f99 100755..100644
--- a/apps/attitude_estimator_ekf/Makefile
+++ b/src/modules/attitude_estimator_ekf/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -31,11 +31,13 @@
#
############################################################################
-APPNAME = attitude_estimator_ekf
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+#
+# Attitude estimator (Extended Kalman Filter)
+#
+
+MODULE_COMMAND = attitude_estimator_ekf
-CSRCS = attitude_estimator_ekf_main.c \
+SRCS = attitude_estimator_ekf_main.cpp \
attitude_estimator_ekf_params.c \
codegen/eye.c \
codegen/attitudeKalmanfilter.c \
@@ -47,10 +49,4 @@ CSRCS = attitude_estimator_ekf_main.c \
codegen/rtGetInf.c \
codegen/rtGetNaN.c \
codegen/norm.c \
- codegen/cross.c
-
-
-# XXX this is *horribly* broken
-INCLUDES += $(TOPDIR)/../mavlink/include/mavlink
-
-include $(APPDIR)/mk/app.mk
+ codegen/cross.c
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c
new file mode 100644
index 000000000..231739962
--- /dev/null
+++ b/src/modules/commander/accelerometer_calibration.c
@@ -0,0 +1,404 @@
+/*
+ * accelerometer_calibration.c
+ *
+ * Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ * Author: Anton Babushkin <rk3dov@gmail.com>
+ *
+ * Transform acceleration vector to true orientation and scale
+ *
+ * * * * Model * * *
+ * accel_corr = accel_T * (accel_raw - accel_offs)
+ *
+ * accel_corr[3] - fully corrected acceleration vector in body frame
+ * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform
+ * accel_raw[3] - raw acceleration vector
+ * accel_offs[3] - acceleration offset vector
+ *
+ * * * * Calibration * * *
+ *
+ * Reference vectors
+ * accel_corr_ref[6][3] = [ g 0 0 ] // nose up
+ * | -g 0 0 | // nose down
+ * | 0 g 0 | // left side down
+ * | 0 -g 0 | // right side down
+ * | 0 0 g | // on back
+ * [ 0 0 -g ] // level
+ * accel_raw_ref[6][3]
+ *
+ * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5
+ *
+ * 6 reference vectors * 3 axes = 18 equations
+ * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
+ *
+ * Find accel_offs
+ *
+ * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
+ *
+ *
+ * Find accel_T
+ *
+ * 9 unknown constants
+ * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
+ *
+ * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
+ *
+ * Solve separate system for each row of accel_T:
+ *
+ * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
+ *
+ * A * x = b
+ *
+ * x = [ accel_T[0][i] ]
+ * | accel_T[1][i] |
+ * [ accel_T[2][i] ]
+ *
+ * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough
+ * | accel_corr_ref[2][i] |
+ * [ accel_corr_ref[4][i] ]
+ *
+ * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
+ *
+ * Matrix A is common for all three systems:
+ * A = [ a[0][0] a[0][1] a[0][2] ]
+ * | a[2][0] a[2][1] a[2][2] |
+ * [ a[4][0] a[4][1] a[4][2] ]
+ *
+ * x = A^-1 * b
+ *
+ * accel_T = A^-1 * g
+ * g = 9.80665
+ */
+
+#include "accelerometer_calibration.h"
+
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_accel.h>
+#include <systemlib/conversions.h>
+#include <mavlink/mavlink_log.h>
+
+void do_accel_calibration(int mavlink_fd);
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
+int detect_orientation(int mavlink_fd, int sub_sensor_combined);
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
+int mat_invert3(float src[3][3], float dst[3][3]);
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
+
+void do_accel_calibration(int mavlink_fd) {
+ /* announce change */
+ mavlink_log_info(mavlink_fd, "accel calibration started");
+
+ /* measure and calculate offsets & scales */
+ float accel_offs[3];
+ float accel_scale[3];
+ int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
+
+ if (res == OK) {
+ /* measurements complete successfully, set parameters */
+ if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
+ || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
+ || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
+ || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
+ || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
+ || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
+ mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
+ }
+
+ int fd = open(ACCEL_DEVICE_PATH, 0);
+ struct accel_scale ascale = {
+ accel_offs[0],
+ accel_scale[0],
+ accel_offs[1],
+ accel_scale[1],
+ accel_offs[2],
+ accel_scale[2],
+ };
+
+ if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
+ warn("WARNING: failed to set scale / offsets for accel");
+
+ close(fd);
+
+ /* auto-save to EEPROM */
+ int save_ret = param_save_default();
+
+ if (save_ret != 0) {
+ warn("WARNING: auto-save of params to storage failed");
+ }
+
+ mavlink_log_info(mavlink_fd, "accel calibration done");
+ tune_positive();
+ } else {
+ /* measurements error */
+ mavlink_log_info(mavlink_fd, "accel calibration aborted");
+ tune_negative();
+ }
+
+ /* exit accel calibration mode */
+}
+
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
+ const int samples_num = 2500;
+ float accel_ref[6][3];
+ bool data_collected[6] = { false, false, false, false, false, false };
+ const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
+
+ /* reset existing calibration */
+ int fd = open(ACCEL_DEVICE_PATH, 0);
+ struct accel_scale ascale_null = {
+ 0.0f,
+ 1.0f,
+ 0.0f,
+ 1.0f,
+ 0.0f,
+ 1.0f,
+ };
+ int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null);
+ close(fd);
+
+ if (OK != ioctl_res) {
+ warn("ERROR: failed to set scale / offsets for accel");
+ return ERROR;
+ }
+
+ int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+ while (true) {
+ bool done = true;
+ char str[80];
+ int str_ptr;
+ str_ptr = sprintf(str, "keep vehicle still:");
+ for (int i = 0; i < 6; i++) {
+ if (!data_collected[i]) {
+ str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]);
+ done = false;
+ }
+ }
+ if (done)
+ break;
+ mavlink_log_info(mavlink_fd, str);
+
+ int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
+ if (orient < 0)
+ return ERROR;
+
+ sprintf(str, "meas started: %s", orientation_strs[orient]);
+ mavlink_log_info(mavlink_fd, str);
+ read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
+ str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
+ mavlink_log_info(mavlink_fd, str);
+ data_collected[orient] = true;
+ tune_neutral();
+ }
+ close(sensor_combined_sub);
+
+ /* calculate offsets and rotation+scale matrix */
+ float accel_T[3][3];
+ int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
+ if (res != 0) {
+ mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error");
+ return ERROR;
+ }
+
+ /* convert accel transform matrix to scales,
+ * rotation part of transform matrix is not used by now
+ */
+ for (int i = 0; i < 3; i++) {
+ accel_scale[i] = accel_T[i][i];
+ }
+
+ return OK;
+}
+
+/*
+ * Wait for vehicle become still and detect it's orientation.
+ *
+ * @return 0..5 according to orientation when vehicle is still and ready for measurements,
+ * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
+ */
+int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
+ struct sensor_combined_s sensor;
+ /* exponential moving average of accel */
+ float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
+ /* max-hold dispersion of accel */
+ float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
+ /* EMA time constant in seconds*/
+ float ema_len = 0.2f;
+ /* set "still" threshold to 0.1 m/s^2 */
+ float still_thr2 = pow(0.1f, 2);
+ /* set accel error threshold to 5m/s^2 */
+ float accel_err_thr = 5.0f;
+ /* still time required in us */
+ int64_t still_time = 2000000;
+ struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+
+ hrt_abstime t_start = hrt_absolute_time();
+ /* set timeout to 30s */
+ hrt_abstime timeout = 30000000;
+ hrt_abstime t_timeout = t_start + timeout;
+ hrt_abstime t = t_start;
+ hrt_abstime t_prev = t_start;
+ hrt_abstime t_still = 0;
+ while (true) {
+ /* wait blocking for new data */
+ int poll_ret = poll(fds, 1, 1000);
+ if (poll_ret) {
+ orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
+ t = hrt_absolute_time();
+ float dt = (t - t_prev) / 1000000.0f;
+ t_prev = t;
+ float w = dt / ema_len;
+ for (int i = 0; i < 3; i++) {
+ accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w;
+ float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i];
+ d = d * d;
+ accel_disp[i] = accel_disp[i] * (1.0f - w);
+ if (d > accel_disp[i])
+ accel_disp[i] = d;
+ }
+ /* still detector with hysteresis */
+ if ( accel_disp[0] < still_thr2 &&
+ accel_disp[1] < still_thr2 &&
+ accel_disp[2] < still_thr2 ) {
+ /* is still now */
+ if (t_still == 0) {
+ /* first time */
+ mavlink_log_info(mavlink_fd, "still...");
+ t_still = t;
+ t_timeout = t + timeout;
+ } else {
+ /* still since t_still */
+ if ((int64_t) t - (int64_t) t_still > still_time) {
+ /* vehicle is still, exit from the loop to detection of its orientation */
+ break;
+ }
+ }
+ } else if ( accel_disp[0] > still_thr2 * 2.0f ||
+ accel_disp[1] > still_thr2 * 2.0f ||
+ accel_disp[2] > still_thr2 * 2.0f) {
+ /* not still, reset still start time */
+ if (t_still != 0) {
+ mavlink_log_info(mavlink_fd, "moving...");
+ t_still = 0;
+ }
+ }
+ } else if (poll_ret == 0) {
+ /* any poll failure for 1s is a reason to abort */
+ mavlink_log_info(mavlink_fd, "ERROR: poll failure");
+ return -3;
+ }
+ if (t > t_timeout) {
+ mavlink_log_info(mavlink_fd, "ERROR: timeout");
+ return -1;
+ }
+ }
+
+ if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+ fabs(accel_ema[1]) < accel_err_thr &&
+ fabs(accel_ema[2]) < accel_err_thr )
+ return 0; // [ g, 0, 0 ]
+ if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+ fabs(accel_ema[1]) < accel_err_thr &&
+ fabs(accel_ema[2]) < accel_err_thr )
+ return 1; // [ -g, 0, 0 ]
+ if ( fabs(accel_ema[0]) < accel_err_thr &&
+ fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+ fabs(accel_ema[2]) < accel_err_thr )
+ return 2; // [ 0, g, 0 ]
+ if ( fabs(accel_ema[0]) < accel_err_thr &&
+ fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+ fabs(accel_ema[2]) < accel_err_thr )
+ return 3; // [ 0, -g, 0 ]
+ if ( fabs(accel_ema[0]) < accel_err_thr &&
+ fabs(accel_ema[1]) < accel_err_thr &&
+ fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr )
+ return 4; // [ 0, 0, g ]
+ if ( fabs(accel_ema[0]) < accel_err_thr &&
+ fabs(accel_ema[1]) < accel_err_thr &&
+ fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr )
+ return 5; // [ 0, 0, -g ]
+
+ mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
+
+ return -2; // Can't detect orientation
+}
+
+/*
+ * Read specified number of accelerometer samples, calculate average and dispersion.
+ */
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
+ struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
+ int count = 0;
+ float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
+
+ while (count < samples_num) {
+ int poll_ret = poll(fds, 1, 1000);
+ if (poll_ret == 1) {
+ struct sensor_combined_s sensor;
+ orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+ for (int i = 0; i < 3; i++)
+ accel_sum[i] += sensor.accelerometer_m_s2[i];
+ count++;
+ } else {
+ return ERROR;
+ }
+ }
+
+ for (int i = 0; i < 3; i++) {
+ accel_avg[i] = accel_sum[i] / count;
+ }
+
+ return OK;
+}
+
+int mat_invert3(float src[3][3], float dst[3][3]) {
+ float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
+ src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
+ src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
+ if (det == 0.0)
+ return ERROR; // Singular matrix
+
+ dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
+ dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
+ dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
+ dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
+ dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
+ dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
+ dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
+ dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
+ dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
+
+ return OK;
+}
+
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) {
+ /* calculate offsets */
+ for (int i = 0; i < 3; i++) {
+ accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
+ }
+
+ /* fill matrix A for linear equations system*/
+ float mat_A[3][3];
+ memset(mat_A, 0, sizeof(mat_A));
+ for (int i = 0; i < 3; i++) {
+ for (int j = 0; j < 3; j++) {
+ float a = accel_ref[i * 2][j] - accel_offs[j];
+ mat_A[i][j] = a;
+ }
+ }
+
+ /* calculate inverse matrix for A */
+ float mat_A_inv[3][3];
+ if (mat_invert3(mat_A, mat_A_inv) != OK)
+ return ERROR;
+
+ /* copy results to accel_T */
+ for (int i = 0; i < 3; i++) {
+ for (int j = 0; j < 3; j++) {
+ /* simplify matrices mult because b has only one non-zero element == g at index i */
+ accel_T[j][i] = mat_A_inv[j][i] * g;
+ }
+ }
+
+ return OK;
+}
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
new file mode 100644
index 000000000..6a920c4ef
--- /dev/null
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -0,0 +1,16 @@
+/*
+ * accelerometer_calibration.h
+ *
+ * Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ * Author: Anton Babushkin <rk3dov@gmail.com>
+ */
+
+#ifndef ACCELEROMETER_CALIBRATION_H_
+#define ACCELEROMETER_CALIBRATION_H_
+
+#include <stdint.h>
+#include <uORB/topics/vehicle_status.h>
+
+void do_accel_calibration(int mavlink_fd);
+
+#endif /* ACCELEROMETER_CALIBRATION_H_ */
diff --git a/apps/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c
index a26938637..a26938637 100644
--- a/apps/commander/calibration_routines.c
+++ b/src/modules/commander/calibration_routines.c
diff --git a/apps/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h
index e3e7fbafd..e3e7fbafd 100644
--- a/apps/commander/calibration_routines.h
+++ b/src/modules/commander/calibration_routines.h
diff --git a/apps/commander/commander.c b/src/modules/commander/commander.c
index 77853aa7a..a34e526a8 100644
--- a/apps/commander/commander.c
+++ b/src/modules/commander/commander.c
@@ -97,7 +97,7 @@
#include <drivers/drv_baro.h>
#include "calibration_routines.h"
-
+#include "accelerometer_calibration.h"
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */
@@ -184,7 +184,6 @@ void do_reboot(void);
void do_gyro_calibration(void);
void do_mag_calibration(void);
void do_rc_calibration(void);
-void do_accel_calibration(void);
void do_airspeed_calibration(void);
@@ -657,113 +656,6 @@ void do_gyro_calibration()
close(sub_sensor_combined);
}
-void do_accel_calibration()
-{
- /* give directions */
- mavlink_log_info(mavlink_fd, "accel calibration starting, keep it level");
-
- const int calibration_count = 2500;
-
- int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
- struct sensor_combined_s raw;
-
- int calibration_counter = 0;
- float accel_offset[3] = {0.0f, 0.0f, 0.0f};
-
- int fd = open(ACCEL_DEVICE_PATH, 0);
- struct accel_scale ascale_null = {
- 0.0f,
- 1.0f,
- 0.0f,
- 1.0f,
- 0.0f,
- 1.0f,
- };
-
- if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null))
- warn("WARNING: failed to set scale / offsets for accel");
-
- close(fd);
-
- while (calibration_counter < calibration_count) {
-
- /* wait blocking for new data */
- struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
-
- int poll_ret = poll(fds, 1, 1000);
-
- if (poll_ret) {
- orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
- accel_offset[0] += raw.accelerometer_m_s2[0];
- accel_offset[1] += raw.accelerometer_m_s2[1];
- accel_offset[2] += raw.accelerometer_m_s2[2];
- calibration_counter++;
-
- } else if (poll_ret == 0) {
- /* any poll failure for 1s is a reason to abort */
- mavlink_log_info(mavlink_fd, "acceleration calibration aborted");
- return;
- }
- }
-
- accel_offset[0] = accel_offset[0] / calibration_count;
- accel_offset[1] = accel_offset[1] / calibration_count;
- accel_offset[2] = accel_offset[2] / calibration_count;
-
- if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) {
-
- /* add the removed length from x / y to z, since we induce a scaling issue else */
- float total_len = sqrtf(accel_offset[0] * accel_offset[0] + accel_offset[1] * accel_offset[1] + accel_offset[2] * accel_offset[2]);
-
- /* if length is correct, zero results here */
- accel_offset[2] = accel_offset[2] + total_len;
-
- float scale = 9.80665f / total_len;
-
- if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))
- || param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))
- || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))
- || param_set(param_find("SENS_ACC_XSCALE"), &(scale))
- || param_set(param_find("SENS_ACC_YSCALE"), &(scale))
- || param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) {
- mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!");
- }
-
- fd = open(ACCEL_DEVICE_PATH, 0);
- struct accel_scale ascale = {
- accel_offset[0],
- scale,
- accel_offset[1],
- scale,
- accel_offset[2],
- scale,
- };
-
- if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
- warn("WARNING: failed to set scale / offsets for accel");
-
- close(fd);
-
- /* auto-save to EEPROM */
- int save_ret = param_save_default();
-
- if (save_ret != 0) {
- warn("WARNING: auto-save of params to storage failed");
- }
-
- //char buf[50];
- //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
- //mavlink_log_info(mavlink_fd, buf);
- mavlink_log_info(mavlink_fd, "accel calibration done");
-
- tune_positive();
-
- } else {
- mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)");
- }
-
- close(sub_sensor_combined);
-}
void do_airspeed_calibration()
{
@@ -772,22 +664,22 @@ void do_airspeed_calibration()
const int calibration_count = 2500;
- int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure));
- struct differential_pressure_s differential_pressure;
+ int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+ struct differential_pressure_s diff_pres;
int calibration_counter = 0;
- float airspeed_offset = 0.0f;
+ float diff_pres_offset = 0.0f;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
- struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } };
+ struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
- orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure);
- airspeed_offset += differential_pressure.voltage;
+ orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+ diff_pres_offset += diff_pres.differential_pressure_pa;
calibration_counter++;
} else if (poll_ret == 0) {
@@ -797,11 +689,11 @@ void do_airspeed_calibration()
}
}
- airspeed_offset = airspeed_offset / calibration_count;
+ diff_pres_offset = diff_pres_offset / calibration_count;
- if (isfinite(airspeed_offset)) {
+ if (isfinite(diff_pres_offset)) {
- if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) {
+ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
}
@@ -823,7 +715,7 @@ void do_airspeed_calibration()
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
}
- close(sub_differential_pressure);
+ close(diff_pres_sub);
}
void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
@@ -925,129 +817,128 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
// break;
//
/* preflight calibration */
- case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
+ case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
- /* gyro calibration */
- if ((int)(cmd->param1) == 1) {
+ /* gyro calibration */
+ if ((int)(cmd->param1) == 1) {
- /* check if no other task is scheduled */
- if(low_prio_task == LOW_PRIO_TASK_NONE) {
+ /* check if no other task is scheduled */
+ if(low_prio_task == LOW_PRIO_TASK_NONE) {
- /* try to go to INIT/PREFLIGHT arming state */
- if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
- /* now set the task for the low prio thread */
- low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION;
- } else {
- result = VEHICLE_CMD_RESULT_DENIED;
- }
+ /* try to go to INIT/PREFLIGHT arming state */
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ /* now set the task for the low prio thread */
+ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION;
} else {
- result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ result = VEHICLE_CMD_RESULT_DENIED;
}
+ } else {
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
+ }
- /* magnetometer calibration */
- if ((int)(cmd->param2) == 1) {
+ /* magnetometer calibration */
+ if ((int)(cmd->param2) == 1) {
- /* check if no other task is scheduled */
- if(low_prio_task == LOW_PRIO_TASK_NONE) {
+ /* check if no other task is scheduled */
+ if(low_prio_task == LOW_PRIO_TASK_NONE) {
- /* try to go to INIT/PREFLIGHT arming state */
- if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
- /* now set the task for the low prio thread */
- low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION;
- } else {
- result = VEHICLE_CMD_RESULT_DENIED;
- }
+ /* try to go to INIT/PREFLIGHT arming state */
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ /* now set the task for the low prio thread */
+ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION;
} else {
- result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ result = VEHICLE_CMD_RESULT_DENIED;
}
+ } else {
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
+ }
-#if 0
- /* zero-altitude pressure calibration */
- if ((int)(cmd->param3) == 1) {
- /* check if no other task is scheduled */
- if(low_prio_task == LOW_PRIO_TASK_NONE) {
+ // /* zero-altitude pressure calibration */
+ // if ((int)(cmd->param3) == 1) {
- /* try to go to INIT/PREFLIGHT arming state */
- if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
- /* now set the task for the low prio thread */
- low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION;
- } else {
- result = VEHICLE_CMD_RESULT_DENIED;
- }
- } else {
- result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
- }
- }
-#endif
-
-#if 0
- /* trim calibration */
- if ((int)(cmd->param4) == 1) {
-
- /* check if no other task is scheduled */
- if(low_prio_task == LOW_PRIO_TASK_NONE) {
-
- /* try to go to INIT/PREFLIGHT arming state */
- if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
- /* now set the task for the low prio thread */
- low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION;
- } else {
- result = VEHICLE_CMD_RESULT_DENIED;
- }
- } else {
- result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
- }
- }
-#endif
+ // /* check if no other task is scheduled */
+ // if(low_prio_task == LOW_PRIO_TASK_NONE) {
- /* accel calibration */
- if ((int)(cmd->param5) == 1) {
+ // /* try to go to INIT/PREFLIGHT arming state */
+ // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+ // result = VEHICLE_CMD_RESULT_ACCEPTED;
+ // /* now set the task for the low prio thread */
+ // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION;
+ // } else {
+ // result = VEHICLE_CMD_RESULT_DENIED;
+ // }
+ // } else {
+ // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ // }
+ // }
- /* check if no other task is scheduled */
- if(low_prio_task == LOW_PRIO_TASK_NONE) {
- /* try to go to INIT/PREFLIGHT arming state */
- if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
- /* now set the task for the low prio thread */
- low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION;
- } else {
- result = VEHICLE_CMD_RESULT_DENIED;
- }
+
+ // /* trim calibration */
+ // if ((int)(cmd->param4) == 1) {
+
+ // /* check if no other task is scheduled */
+ // if(low_prio_task == LOW_PRIO_TASK_NONE) {
+
+ // /* try to go to INIT/PREFLIGHT arming state */
+ // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+ // result = VEHICLE_CMD_RESULT_ACCEPTED;
+ // /* now set the task for the low prio thread */
+ // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION;
+ // } else {
+ // result = VEHICLE_CMD_RESULT_DENIED;
+ // }
+ // } else {
+ // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ // }
+ // }
+
+
+ /* accel calibration */
+ if ((int)(cmd->param5) == 1) {
+
+ /* check if no other task is scheduled */
+ if(low_prio_task == LOW_PRIO_TASK_NONE) {
+
+ /* try to go to INIT/PREFLIGHT arming state */
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ /* now set the task for the low prio thread */
+ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION;
} else {
- result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ result = VEHICLE_CMD_RESULT_DENIED;
}
+ } else {
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
+ }
- /* airspeed calibration */
- if ((int)(cmd->param6) == 1) {
+ /* airspeed calibration */
+ if ((int)(cmd->param6) == 1) {
- /* check if no other task is scheduled */
- if(low_prio_task == LOW_PRIO_TASK_NONE) {
+ /* check if no other task is scheduled */
+ if(low_prio_task == LOW_PRIO_TASK_NONE) {
- /* try to go to INIT/PREFLIGHT arming state */
- if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
- /* now set the task for the low prio thread */
- low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION;
- } else {
- result = VEHICLE_CMD_RESULT_DENIED;
- }
+ /* try to go to INIT/PREFLIGHT arming state */
+ if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ /* now set the task for the low prio thread */
+ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION;
} else {
- result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ result = VEHICLE_CMD_RESULT_DENIED;
}
+ } else {
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
- case VEHICLE_CMD_PREFLIGHT_STORAGE: {
+ case VEHICLE_CMD_PREFLIGHT_STORAGE:
if (((int)(cmd->param1)) == 0) {
@@ -1070,12 +961,11 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
- } break;
+ break;
- default: {
- mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
- result = VEHICLE_CMD_RESULT_UNSUPPORTED;
- }
+ default:
+ mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
+ result = VEHICLE_CMD_RESULT_UNSUPPORTED;
break;
}
@@ -1422,10 +1312,10 @@ int commander_thread_main(int argc, char *argv[])
struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors));
- int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
- struct differential_pressure_s differential_pressure;
- memset(&differential_pressure, 0, sizeof(differential_pressure));
- uint64_t last_differential_pressure_time = 0;
+ int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+ struct differential_pressure_s diff_pres;
+ memset(&diff_pres, 0, sizeof(diff_pres));
+ uint64_t last_diff_pres_time = 0;
/* Subscribe to command topic */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -1482,11 +1372,11 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
}
- orb_check(differential_pressure_sub, &new_data);
+ orb_check(diff_pres_sub, &new_data);
if (new_data) {
- orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
- last_differential_pressure_time = differential_pressure.timestamp;
+ orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+ last_diff_pres_time = diff_pres.timestamp;
}
orb_check(cmd_sub, &new_data);
@@ -1574,26 +1464,46 @@ int commander_thread_main(int argc, char *argv[])
/* Slow but important 8 Hz checks */
if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
- /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
-// if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
-// current_status.state_machine == SYSTEM_STATE_AUTO ||
-// current_status.state_machine == SYSTEM_STATE_MANUAL)) {
-// /* armed */
- led_toggle(LED_BLUE);
-
- } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
- /* not armed */
- led_toggle(LED_BLUE);
- }
- /* toggle error led at 5 Hz in HIL mode */
- if (current_status.flag_hil_enabled) {
- /* hil enabled */
- led_toggle(LED_AMBER);
+ /* XXX if armed */
+ if ((false /*current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
+ current_status.state_machine == SYSTEM_STATE_AUTO ||
+ current_status.state_machine == SYSTEM_STATE_MANUAL*/)) {
+ /* armed, solid */
+ led_on(LED_AMBER);
+
+ } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+ /* not armed */
+ led_toggle(LED_AMBER);
+ }
+
+ if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
+
+ /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
+ if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
+ && (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
+ /* GPS lock */
+ led_on(LED_BLUE);
- } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
- /* toggle error (red) at 5 Hz on low battery or error */
- led_toggle(LED_AMBER);
+ } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+ /* no GPS lock, but GPS module is aquiring lock */
+ led_toggle(LED_BLUE);
+ }
+
+ } else {
+ /* no GPS info, don't light the blue led */
+ led_off(LED_BLUE);
+ }
+
+ /* toggle GPS led at 5 Hz in HIL mode */
+ if (current_status.flag_hil_enabled) {
+ /* hil enabled */
+ led_toggle(LED_BLUE);
+
+ } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
+ /* toggle arming (red) at 5 Hz on low battery or error */
+ led_toggle(LED_AMBER);
+ }
}
@@ -1654,7 +1564,7 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
// XXX implement this
-// state_machine_emergency(stat_pub, &current_status, mavlink_fd);
+ // state_machine_emergency(stat_pub, &current_status, mavlink_fd);
}
critical_voltage_counter++;
@@ -1709,7 +1619,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* Check for valid airspeed/differential pressure measurements */
- if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
+ if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
current_status.condition_airspeed_valid = true;
} else {
@@ -2228,13 +2138,13 @@ int commander_thread_main(int argc, char *argv[])
// XXX this is missing
/* If full run came back clean, transition to standby */
-// if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
-// current_status.flag_preflight_gyro_calibration == false &&
-// current_status.flag_preflight_mag_calibration == false &&
-// current_status.flag_preflight_accel_calibration == false) {
-// /* All ok, no calibration going on, go to standby */
-// do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-// }
+ // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
+ // current_status.flag_preflight_gyro_calibration == false &&
+ // current_status.flag_preflight_mag_calibration == false &&
+ // current_status.flag_preflight_accel_calibration == false) {
+ // /* All ok, no calibration going on, go to standby */
+ // do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
+ // }
/* publish at least with 1 Hz */
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) {
@@ -2323,18 +2233,18 @@ void *commander_low_prio_loop(void *arg)
case LOW_PRIO_TASK_ALTITUDE_CALIBRATION:
-// do_baro_calibration();
+ // do_baro_calibration();
case LOW_PRIO_TASK_RC_CALIBRATION:
-// do_rc_calibration();
+ // do_rc_calibration();
low_prio_task = LOW_PRIO_TASK_NONE;
break;
case LOW_PRIO_TASK_ACCEL_CALIBRATION:
- do_accel_calibration();
+ do_accel_calibration(mavlink_fd);
low_prio_task = LOW_PRIO_TASK_NONE;
break;
diff --git a/apps/commander/commander.h b/src/modules/commander/commander.h
index 04b4e72ab..04b4e72ab 100644
--- a/apps/commander/commander.h
+++ b/src/modules/commander/commander.h
diff --git a/apps/drivers/boards/px4fmu/Makefile b/src/modules/commander/module.mk
index 6b183d8d2..fe44e955a 100644
--- a/apps/drivers/boards/px4fmu/Makefile
+++ b/src/modules/commander/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,10 +32,12 @@
############################################################################
#
-# Board-specific startup code for the PX4FMU
+# Main system state machine
#
-INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-LIBNAME = brd_px4fmu
+MODULE_COMMAND = commander
+SRCS = commander.c \
+ state_machine_helper.c \
+ calibration_routines.c \
+ accelerometer_calibration.c
-include $(APPDIR)/mk/app.mk
diff --git a/apps/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c
index ba01f8410..daed81553 100644
--- a/apps/commander/state_machine_helper.c
+++ b/src/modules/commander/state_machine_helper.c
@@ -557,17 +557,20 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
}
-//void publish_armed_status(const struct vehicle_status_s *current_status)
-//{
-// struct actuator_armed_s armed;
-// armed.armed = current_status->flag_fmu_armed;
+// void publish_armed_status(const struct vehicle_status_s *current_status)
+// {
+// struct actuator_armed_s armed;
+// armed.armed = current_status->flag_system_armed;
+//
+// /* XXX allow arming by external components on multicopters only if not yet armed by RC */
+// /* XXX allow arming only if core sensors are ok */
+// armed.ready_to_arm = true;
+//
// /* lock down actuators if required, only in HIL */
-// armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
-// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
-// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
-//}
-
-
+// armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
+// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+// }
// /*
diff --git a/apps/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 5b57cffb7..5b57cffb7 100644
--- a/apps/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
diff --git a/apps/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp
index 5994d2315..5994d2315 100644
--- a/apps/controllib/block/Block.cpp
+++ b/src/modules/controllib/block/Block.cpp
diff --git a/apps/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp
index 258701f27..258701f27 100644
--- a/apps/controllib/block/Block.hpp
+++ b/src/modules/controllib/block/Block.hpp
diff --git a/apps/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp
index fd12e365d..fd12e365d 100644
--- a/apps/controllib/block/BlockParam.cpp
+++ b/src/modules/controllib/block/BlockParam.cpp
diff --git a/apps/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp
index 58a9bfc0d..58a9bfc0d 100644
--- a/apps/controllib/block/BlockParam.hpp
+++ b/src/modules/controllib/block/BlockParam.hpp
diff --git a/apps/controllib/block/List.hpp b/src/modules/controllib/block/List.hpp
index 96b0b94d1..96b0b94d1 100644
--- a/apps/controllib/block/List.hpp
+++ b/src/modules/controllib/block/List.hpp
diff --git a/apps/controllib/block/UOrbPublication.cpp b/src/modules/controllib/block/UOrbPublication.cpp
index f69b39d90..f69b39d90 100644
--- a/apps/controllib/block/UOrbPublication.cpp
+++ b/src/modules/controllib/block/UOrbPublication.cpp
diff --git a/apps/controllib/block/UOrbPublication.hpp b/src/modules/controllib/block/UOrbPublication.hpp
index a36f4429f..a36f4429f 100644
--- a/apps/controllib/block/UOrbPublication.hpp
+++ b/src/modules/controllib/block/UOrbPublication.hpp
diff --git a/apps/controllib/block/UOrbSubscription.cpp b/src/modules/controllib/block/UOrbSubscription.cpp
index 022cadd24..022cadd24 100644
--- a/apps/controllib/block/UOrbSubscription.cpp
+++ b/src/modules/controllib/block/UOrbSubscription.cpp
diff --git a/apps/controllib/block/UOrbSubscription.hpp b/src/modules/controllib/block/UOrbSubscription.hpp
index 22cc2e114..22cc2e114 100644
--- a/apps/controllib/block/UOrbSubscription.hpp
+++ b/src/modules/controllib/block/UOrbSubscription.hpp
diff --git a/apps/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index c6c374300..c6c374300 100644
--- a/apps/controllib/blocks.cpp
+++ b/src/modules/controllib/blocks.cpp
diff --git a/apps/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 7a785d12e..7a785d12e 100644
--- a/apps/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
diff --git a/apps/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp
index a93181f16..8b0ea2fac 100644
--- a/apps/controllib/fixedwing.cpp
+++ b/src/modules/controllib/fixedwing.cpp
@@ -57,9 +57,9 @@ BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
BlockYawDamper::~BlockYawDamper() {};
-void BlockYawDamper::update(float rCmd, float r)
+void BlockYawDamper::update(float rCmd, float r, float outputScale)
{
- _rudder = _r2Rdr.update(rCmd -
+ _rudder = outputScale*_r2Rdr.update(rCmd -
_rWashout.update(_rLowPass.update(r)));
}
@@ -78,13 +78,13 @@ BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
BlockStabilization::~BlockStabilization() {};
void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
- float p, float q, float r)
+ float p, float q, float r, float outputScale)
{
- _aileron = _p2Ail.update(
+ _aileron = outputScale*_p2Ail.update(
pCmd - _pLowPass.update(p));
- _elevator = _q2Elv.update(
+ _elevator = outputScale*_q2Elv.update(
qCmd - _qLowPass.update(q));
- _yawDamper.update(rCmd, r);
+ _yawDamper.update(rCmd, r, outputScale);
}
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
@@ -157,21 +157,21 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
_theLimit(this, "THE"),
_vLimit(this, "V"),
- // altitude/roc hold
+ // altitude/climb rate hold
_h2Thr(this, "H2THR"),
- _roc2Thr(this, "ROC2THR"),
+ _cr2Thr(this, "CR2THR"),
// guidance block
_guide(this, ""),
- // block params
- _trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
- _trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
- _trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
- _trimThr(this, "TRIM_THR", true), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
+ _trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
+ _trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
+ _trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
+ _trimThr(this, "TRIM_THR"), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
+ _trimV(this, "TRIM_V"), /* FWB_ specific trim velocity (full name : FWB_TRIM_V) */
_vCmd(this, "V_CMD"),
- _rocMax(this, "ROC_MAX"),
+ _crMax(this, "CR_MAX"),
_attPoll(),
_lastPosCmd(),
_timeStamp(0)
@@ -231,7 +231,15 @@ void BlockMultiModeBacksideAutopilot::update()
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
// calculate velocity, XXX should be airspeed, but using ground speed for now
- float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
+ // for the purpose of control we will limit the velocity feedback between
+ // the min/max velocity
+ float v = _vLimit.update(sqrtf(
+ _pos.vx * _pos.vx +
+ _pos.vy * _pos.vy +
+ _pos.vz * _pos.vz));
+
+ // limit velocity command between min/max velocity
+ float vCmd = _vLimit.update(_vCmd.get());
// altitude hold
float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
@@ -243,16 +251,19 @@ void BlockMultiModeBacksideAutopilot::update()
// velocity hold
// negative sign because nose over to increase speed
- float thetaCmd = _theLimit.update(-_v2Theta.update(
- _vLimit.update(_vCmd.get()) - v));
+ float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd
float rCmd = 0;
// stabilization
+ float velocityRatio = _trimV.get()/v;
+ float outputScale = velocityRatio*velocityRatio;
+ // this term scales the output based on the dynamic pressure change from trim
_stabilization.update(pCmd, qCmd, rCmd,
- _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+ _att.rollspeed, _att.pitchspeed, _att.yawspeed,
+ outputScale);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
@@ -284,13 +295,18 @@ void BlockMultiModeBacksideAutopilot::update()
// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
- float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
+ // for the purpose of control we will limit the velocity feedback between
+ // the min/max velocity
+ float v = _vLimit.update(sqrtf(
+ _pos.vx * _pos.vx +
+ _pos.vy * _pos.vy +
+ _pos.vz * _pos.vz));
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
// from +1 -> -1 m/s for rate of climb
- //float dThrottle = _roc2Thr.update(
- //_rocMax.get()*_manual.pitch - _pos.vz);
+ //float dThrottle = _cr2Thr.update(
+ //_crMax.get()*_manual.pitch - _pos.vz);
// roll channel -> bank angle
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
@@ -298,8 +314,10 @@ void BlockMultiModeBacksideAutopilot::update()
// throttle channel -> velocity
// negative sign because nose over to increase speed
- float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
- float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
+ float vCmd = _vLimit.update(_manual.throttle *
+ (_vLimit.getMax() - _vLimit.getMin()) +
+ _vLimit.getMin());
+ float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd
diff --git a/apps/controllib/fixedwing.hpp b/src/modules/controllib/fixedwing.hpp
index 281cbb4cb..53d0cf893 100644
--- a/apps/controllib/fixedwing.hpp
+++ b/src/modules/controllib/fixedwing.hpp
@@ -193,7 +193,7 @@ public:
* good idea to declare a member to store the temporary
* variable.
*/
- void update(float rCmd, float r);
+ void update(float rCmd, float r, float outputScale = 1.0);
/**
* Rudder output value accessor
@@ -226,7 +226,8 @@ public:
BlockStabilization(SuperBlock *parent, const char *name);
virtual ~BlockStabilization();
void update(float pCmd, float qCmd, float rCmd,
- float p, float q, float r);
+ float p, float q, float r,
+ float outputScale = 1.0);
float getAileron() { return _aileron; }
float getElevator() { return _elevator; }
float getRudder() { return _yawDamper.getRudder(); }
@@ -310,9 +311,9 @@ private:
BlockLimit _theLimit;
BlockLimit _vLimit;
- // altitude/ roc hold
+ // altitude/ climb rate hold
BlockPID _h2Thr;
- BlockPID _roc2Thr;
+ BlockPID _cr2Thr;
// guidance
BlockWaypointGuidance _guide;
@@ -322,8 +323,9 @@ private:
BlockParam<float> _trimElv;
BlockParam<float> _trimRdr;
BlockParam<float> _trimThr;
+ BlockParam<float> _trimV;
BlockParam<float> _vCmd;
- BlockParam<float> _rocMax;
+ BlockParam<float> _crMax;
struct pollfd _attPoll;
vehicle_global_position_setpoint_s _lastPosCmd;
diff --git a/apps/controllib/Makefile b/src/modules/controllib/module.mk
index 6749b805f..13d1069c7 100644
--- a/apps/controllib/Makefile
+++ b/src/modules/controllib/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -34,13 +34,10 @@
#
# Control library
#
-CSRCS = test_params.c
-
-CXXSRCS = block/Block.cpp \
+SRCS = test_params.c \
+ block/Block.cpp \
block/BlockParam.cpp \
block/UOrbPublication.cpp \
block/UOrbSubscription.cpp \
blocks.cpp \
fixedwing.cpp
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/controllib/test_params.c b/src/modules/controllib/test_params.c
index 7c609cad3..7c609cad3 100644
--- a/apps/controllib/test_params.c
+++ b/src/modules/controllib/test_params.c
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
index b012448a2..769b8b0a8 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
@@ -35,7 +35,6 @@
* @file fixedwing_att_control_rate.c
* Implementation of a fixed wing attitude controller.
*/
-#include <fixedwing_att_control_att.h>
#include <nuttx/config.h>
#include <stdio.h>
@@ -59,7 +58,7 @@
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
-
+#include "fixedwing_att_control_att.h"
struct fw_att_control_params {
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.h b/src/modules/fixedwing_att_control/fixedwing_att_control_att.h
index 600e35b89..600e35b89 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_att.h
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.h
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
index 12f06cdd2..758c97d78 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
@@ -65,8 +65,9 @@
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
-#include <fixedwing_att_control_rate.h>
-#include <fixedwing_att_control_att.h>
+
+#include "fixedwing_att_control_rate.h"
+#include "fixedwing_att_control_att.h"
/* Prototypes */
/**
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
index ba5b593e2..4eccc118c 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
@@ -33,11 +33,11 @@
****************************************************************************/
/**
* @file fixedwing_att_control_rate.c
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *
* Implementation of a fixed wing attitude controller.
*
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/
-#include <fixedwing_att_control_rate.h>
#include <nuttx/config.h>
#include <stdio.h>
@@ -61,6 +61,8 @@
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
+#include "fixedwing_att_control_rate.h"
+
/*
* Controller parameters, accessible via MAVLink
*
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.h b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h
index 500e3e197..500e3e197 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_rate.h
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h
diff --git a/src/modules/fixedwing_att_control/module.mk b/src/modules/fixedwing_att_control/module.mk
new file mode 100644
index 000000000..fd1a8724a
--- /dev/null
+++ b/src/modules/fixedwing_att_control/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Fixedwing Attitude Control application
+#
+
+MODULE_COMMAND = fixedwing_att_control
+
+SRCS = fixedwing_att_control_main.c \
+ fixedwing_att_control_att.c \
+ fixedwing_att_control_rate.c
diff --git a/apps/examples/control_demo/control_demo.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
index e609f2f4b..e21990c92 100644
--- a/apps/examples/control_demo/control_demo.cpp
+++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
@@ -1,7 +1,7 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: @author Example User <mail@example.com>
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Author: James Goppert
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,8 +33,10 @@
****************************************************************************/
/**
- * @file control_demo.cpp
- * Demonstration of control library
+ * @file fixedwing_backside_main.cpp
+ * @author James Goppert
+ *
+ * Fixedwing backside controller using control library
*/
#include <nuttx/config.h>
@@ -55,7 +57,7 @@ static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
-extern "C" __EXPORT int control_demo_main(int argc, char *argv[]);
+extern "C" __EXPORT int fixedwing_backside_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
@@ -90,7 +92,7 @@ usage(const char *reason)
* The actual stack size should be set in the call
* to task_create().
*/
-int control_demo_main(int argc, char *argv[])
+int fixedwing_backside_main(int argc, char *argv[])
{
if (argc < 1)
diff --git a/src/modules/fixedwing_backside/module.mk b/src/modules/fixedwing_backside/module.mk
new file mode 100644
index 000000000..a9233288b
--- /dev/null
+++ b/src/modules/fixedwing_backside/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Fixedwing backside controller
+#
+
+MODULE_COMMAND = fixedwing_backside
+
+SRCS = fixedwing_backside_main.cpp
diff --git a/apps/examples/control_demo/params.c b/src/modules/fixedwing_backside/params.c
index 428b779b1..db30af416 100644
--- a/apps/examples/control_demo/params.c
+++ b/src/modules/fixedwing_backside/params.c
@@ -59,13 +59,14 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity
// rate of climb
// this is what rate of climb is commanded (in m/s)
// when the pitch stick is fully defelcted in simple mode
-PARAM_DEFINE_FLOAT(FWB_ROC_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f);
-// rate of climb -> thr
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_P, 0.01f); // rate of climb to throttle PID
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_I, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_D, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_D_LP, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_ROC2THR_I_MAX, 0.0f);
+// climb rate -> thr
+PARAM_DEFINE_FLOAT(FWB_CR2THR_P, 0.01f); // rate of climb to throttle PID
+PARAM_DEFINE_FLOAT(FWB_CR2THR_I, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_CR2THR_D, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_CR2THR_D_LP, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_CR2THR_I_MAX, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f); // trim velocity, m/s
diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
index 71c78f5b8..71c78f5b8 100644
--- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
diff --git a/apps/systemcmds/delay_test/Makefile b/src/modules/fixedwing_pos_control/module.mk
index d30fcba27..b976377e9 100644
--- a/apps/systemcmds/delay_test/Makefile
+++ b/src/modules/fixedwing_pos_control/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,11 +32,9 @@
############################################################################
#
-# Delay test
+# Fixedwing PositionControl application
#
-APPNAME = delay_test
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+MODULE_COMMAND = fixedwing_pos_control
-include $(APPDIR)/mk/app.mk
+SRCS = fixedwing_pos_control_main.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_f32.c
index 7070274da..7070274da 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q15.c
index 6d6a4b6d8..6d6a4b6d8 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q31.c
index d9e3e7aa3..d9e3e7aa3 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q7.c
index 258ecef2c..258ecef2c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_f32.c
index 0961dd1db..0961dd1db 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q15.c
index 788b2ebb2..788b2ebb2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q31.c
index c5b18711d..c5b18711d 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q7.c
index 5407c006f..5407c006f 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_f32.c
index 6f7fbcebf..6f7fbcebf 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q15.c
index 24611a05f..24611a05f 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q31.c
index 5e4031338..5e4031338 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q7.c
index ddf2d0c24..ddf2d0c24 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_f32.c
index 88a458b40..88a458b40 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q15.c
index 99d44a4ba..99d44a4ba 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q31.c
index 37ec8e25e..37ec8e25e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q7.c
index 14ee23ea6..14ee23ea6 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_f32.c
index 4f4d423ac..4f4d423ac 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q15.c
index 21d58d1a4..21d58d1a4 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q31.c
index fc28c3621..fc28c3621 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q7.c
index 601fb7393..601fb7393 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_f32.c
index 4bdf06ee8..4bdf06ee8 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q15.c
index f8158f066..f8158f066 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q31.c
index 8b03ee372..8b03ee372 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q7.c
index 56643816f..56643816f 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_f32.c
index b77144a3e..b77144a3e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q15.c
index 9e79202fe..9e79202fe 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q31.c
index 8e9b50e72..8e9b50e72 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q7.c
index 76c35e656..76c35e656 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q15.c
index b8301e63d..b8301e63d 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q31.c
index 561225a89..561225a89 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q7.c
index 6dfd25f65..6dfd25f65 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_f32.c
index 4a660db49..4a660db49 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q15.c
index d8cc12220..d8cc12220 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q31.c
index 34642f32e..34642f32e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q7.c
index adbc7d0c0..adbc7d0c0 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/CommonTables/arm_common_tables.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/CommonTables/arm_common_tables.c
index bf7ab3bed..bf7ab3bed 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/CommonTables/arm_common_tables.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/CommonTables/arm_common_tables.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_f32.c
index 31bbaa7a8..31bbaa7a8 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q15.c
index c5a370315..c5a370315 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q31.c
index 7475962d5..7475962d5 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.c
index 4f265d690..4f265d690 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.c
index 33df820a5..33df820a5 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.c
index d371dff8b..d371dff8b 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
index 59bbc2742..59bbc2742 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q15.c
index 122ca8bc1..122ca8bc1 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c
index 59384f5dc..59384f5dc 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.c
index 7350fd1c9..7350fd1c9 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.c
index 623229b8f..623229b8f 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.c
index d45ccfab2..d45ccfab2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.c
index 2ec9ee862..2ec9ee862 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.c
index 0219aea48..0219aea48 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.c
index 13fc060a2..13fc060a2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.c
index 7a5bdf9cd..7a5bdf9cd 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.c
index a44158c4f..a44158c4f 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.c
index b286acdb6..b286acdb6 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_f32.c
index 37e48f609..37e48f609 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q15.c
index 1816bc9aa..1816bc9aa 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q31.c
index da40ce973..da40ce973 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_f32.c
index 491ad5268..491ad5268 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q15.c
index dc0078746..dc0078746 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q31.c
index b1ff753af..b1ff753af 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_f32.c
index 1679a780c..1679a780c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_q31.c
index e22d25e53..e22d25e53 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_sin_cos_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_f32.c
index bee758bff..bee758bff 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q15.c
index 0edf68b89..0edf68b89 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q31.c
index 1326215fe..1326215fe 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_f32.c
index bfe58d5c8..bfe58d5c8 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q15.c
index b53ca3ecc..b53ca3ecc 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q31.c
index 336796b5c..336796b5c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q15.c
index 0a6d6a53a..0a6d6a53a 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q31.c
index 2a3b7ac18..2a3b7ac18 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sqrt_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_init_q31.c
index a6745c0cd..a6745c0cd 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.c
index 82d6164ee..82d6164ee 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_f32.c
index ee20dfeec..ee20dfeec 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.c
index 29afffa03..29afffa03 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q31.c
index 0a479fe6b..0a479fe6b 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f32.c
index d50b69f3c..d50b69f3c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q15.c
index d5fda28ac..d5fda28ac 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q31.c
index dbbb8aa2b..dbbb8aa2b 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q15.c
index 484cd85e8..484cd85e8 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q31.c
index 5626bdd1c..5626bdd1c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_f32.c
index a8cb0c98c..a8cb0c98c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f32.c
index e4225d008..e4225d008 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_f32.c
index 48dd45f2b..48dd45f2b 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_opt_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_opt_q15.c
index 3e85a25ba..3e85a25ba 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_opt_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_opt_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q15.c
index e21be3523..e21be3523 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q31.c
index e675c11ce..e675c11ce 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_fast_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q15.c
index 70b4125d9..70b4125d9 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q7.c
index 7fe9f55cf..7fe9f55cf 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_opt_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_f32.c
index 58f572735..58f572735 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_opt_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_opt_q15.c
index 25b3ba8c3..25b3ba8c3 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_opt_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_opt_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q15.c
index 98216bb6a..98216bb6a 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q31.c
index 17902f593..17902f593 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_fast_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q15.c
index fe14f9fd3..fe14f9fd3 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q7.c
index 513d89d87..513d89d87 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_opt_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q15.c
index 8c7ed5f86..8c7ed5f86 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q31.c
index 16a060648..16a060648 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q7.c
index ed205838e..ed205838e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_partial_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q15.c
index 1907719e7..1907719e7 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q31.c
index 769b95abb..769b95abb 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q7.c
index eb78fd533..eb78fd533 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_conv_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_f32.c
index 6a99eafc1..6a99eafc1 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_opt_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_opt_q15.c
index a99225a2f..a99225a2f 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_opt_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_opt_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q15.c
index 0c79bc896..0c79bc896 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q31.c
index 628fd3ea5..628fd3ea5 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_fast_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q15.c
index cc33b54f5..cc33b54f5 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q7.c
index 6749b01b3..6749b01b3 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_opt_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q15.c
index fc4134b7b..fc4134b7b 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q31.c
index c81f8600d..c81f8600d 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q7.c
index e03e4997b..e03e4997b 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_correlate_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_f32.c
index 1546f407d..1546f407d 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q15.c
index 96f50e6f2..96f50e6f2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q31.c
index a43fd0b4c..a43fd0b4c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_fast_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_f32.c
index b655a6be6..b655a6be6 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q15.c
index 1a1fee8a2..1a1fee8a2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q31.c
index e2ac8165e..e2ac8165e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q15.c
index 28b5f13dd..28b5f13dd 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q31.c
index 59c0fed7f..59c0fed7f 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_decimate_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_f32.c
index 7f951f86b..7f951f86b 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q15.c
index 59abff2f5..59abff2f5 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q31.c
index e384ff973..e384ff973 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_fast_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_f32.c
index aaeddf5f6..aaeddf5f6 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q15.c
index 02bb33623..02bb33623 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q31.c
index 997a78427..997a78427 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q7.c
index 65a49a823..65a49a823 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_init_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_f32.c
index c966a0ced..c966a0ced 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_f32.c
index b19347264..b19347264 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q15.c
index b344fc785..b344fc785 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q31.c
index f0383b8e9..f0383b8e9 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q15.c
index d17d8d803..d17d8d803 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q31.c
index 689b4d337..689b4d337 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_f32.c
index 3745ffb6a..3745ffb6a 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_f32.c
index a53ae92e2..a53ae92e2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q15.c
index 8db498509..8db498509 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q31.c
index 035c5e056..035c5e056 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q15.c
index abfb33cf2..abfb33cf2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q31.c
index 9fb46459d..9fb46459d 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q15.c
index 368014f43..368014f43 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q31.c
index b3b84ec51..b3b84ec51 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q7.c
index 624afa130..624afa130 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_f32.c
index e969d5c84..e969d5c84 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_f32.c
index 2934352d0..2934352d0 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q15.c
index 47a116e95..47a116e95 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q31.c
index 360a219b2..360a219b2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q7.c
index b400297ca..b400297ca 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_init_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q15.c
index 016f83345..016f83345 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q31.c
index 3d2f6d402..3d2f6d402 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q7.c
index ace5d0703..ace5d0703 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_sparse_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_f32.c
index 074605553..074605553 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_f32.c
index 89c00c24d..89c00c24d 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q15.c
index 7dac99fca..7dac99fca 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q31.c
index 73b18a597..73b18a597 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q15.c
index 5f0f891d1..5f0f891d1 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q31.c
index adfd4dfb5..adfd4dfb5 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_f32.c
index 90fa8ae1e..90fa8ae1e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_f32.c
index a2f51240a..a2f51240a 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q15.c
index 8f42949a6..8f42949a6 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q31.c
index 58edb659b..58edb659b 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_f32.c
index b2ac45206..b2ac45206 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_f32.c
index 3d31cfb25..3d31cfb25 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q15.c
index a1cf1b001..a1cf1b001 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q31.c
index a2fae7b38..a2fae7b38 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q15.c
index a1229a203..a1229a203 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q31.c
index 791a8637c..791a8637c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_norm_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q15.c
index 91237c12d..91237c12d 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q31.c
index c43d55d1d..c43d55d1d 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_lms_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_f32.c
index 09099ccb8..09099ccb8 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q15.c
index 4bfc87862..4bfc87862 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q31.c
index 1f7be21c5..1f7be21c5 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_add_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_f32.c
index 9ffc96ee3..9ffc96ee3 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q15.c
index 2bada7fed..2bada7fed 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q31.c
index 8828c2d2e..8828c2d2e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_inverse_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_inverse_f32.c
index f346916e0..f346916e0 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_inverse_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_inverse_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_f32.c
index a2f513628..a2f513628 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q15.c
index a88e3cdd1..a88e3cdd1 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q31.c
index 7fdbb1cb8..7fdbb1cb8 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_fast_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q15.c
index b48d204ed..b48d204ed 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q31.c
index eb7652204..eb7652204 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_f32.c
index e64cfd1d1..e64cfd1d1 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q15.c
index 58011254c..58011254c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q31.c
index e5e124125..e5e124125 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_scale_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_f32.c
index 3544d8e82..3544d8e82 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q15.c
index 1857fd837..1857fd837 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q31.c
index 279fd7f7c..279fd7f7c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_sub_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_f32.c
index 235028f5a..235028f5a 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q15.c
index be9c78e00..be9c78e00 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q31.c
index 253a923d1..253a923d1 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_trans_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_f32.c
index afd964885..afd964885 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q15.c
index 939b5236f..939b5236f 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q31.c
index 18e757297..18e757297 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q7.c
index 110a8fe5f..110a8fe5f 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_max_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_f32.c
index 7823d746c..7823d746c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q15.c
index 49ec1e62e..49ec1e62e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q31.c
index 88a5de299..88a5de299 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q7.c
index a34c011bc..a34c011bc 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_mean_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_f32.c
index dc2fdf624..dc2fdf624 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q15.c
index 35b67bb1e..35b67bb1e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q31.c
index 4a9befc75..4a9befc75 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q7.c
index aaf8ad9ff..aaf8ad9ff 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_min_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_f32.c
index 5ee0060d8..5ee0060d8 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q15.c
index aed06a17a..aed06a17a 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q31.c
index 43f03f757..43f03f757 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q7.c
index 5cb99b4c2..5cb99b4c2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_f32.c
index 573896ed4..573896ed4 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q15.c
index 491c652e7..491c652e7 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q31.c
index 54038eb12..54038eb12 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_rms_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_f32.c
index 6442d5c29..6442d5c29 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q15.c
index 2d1a49a50..2d1a49a50 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q31.c
index 45bec01a1..45bec01a1 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_std_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_f32.c
index 2adcfb443..2adcfb443 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q15.c
index 08dbc6587..08dbc6587 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q31.c
index 13d6c15cb..13d6c15cb 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_var_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_f32.c
index 619c1577a..619c1577a 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q15.c
index 00be9dc0b..00be9dc0b 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q31.c
index 7ab0849f6..7ab0849f6 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q7.c
index 425885753..425885753 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_copy_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_f32.c
index 439e60db0..439e60db0 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q15.c
index 8f60d3b4e..8f60d3b4e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q31.c
index 35565c3cf..35565c3cf 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q7.c
index 481d4c8f7..481d4c8f7 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_fill_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q15.c
index 1537b93ab..1537b93ab 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q31.c
index 3ab52b9ff..3ab52b9ff 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q7.c
index dea14c6a1..dea14c6a1 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_float_to_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_float.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_float.c
index ce51e9a39..ce51e9a39 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_float.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_float.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q31.c
index a0f66c29e..a0f66c29e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q7.c
index 87fe63d9e..87fe63d9e 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q15_to_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_float.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_float.c
index 94deec9a1..94deec9a1 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_float.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_float.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q15.c
index ba79f85a1..ba79f85a1 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q7.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q7.c
index afc8fdbed..afc8fdbed 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q7.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q31_to_q7.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_float.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_float.c
index be4ac5fe3..be4ac5fe3 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_float.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_float.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q15.c
index 019ca4815..019ca4815 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q31.c
index bbbcc6f4a..bbbcc6f4a 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/SupportFunctions/arm_q7_to_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_bitreversal.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_bitreversal.c
index 3da790ebb..3da790ebb 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_bitreversal.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_bitreversal.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_f32.c
index 274b69928..274b69928 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_f32.c
index a41ed25d2..a41ed25d2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q15.c
index 86aa149b2..86aa149b2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q31.c
index 29aa9a74b..29aa9a74b 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q15.c
index 00c7420cf..00c7420cf 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q31.c
index dd4b9e6b2..dd4b9e6b2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix2_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_f32.c
index d51e9830b..d51e9830b 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_f32.c
index f354e1dde..f354e1dde 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q15.c
index 5ace3483d..5ace3483d 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q31.c
index 6614994f7..6614994f7 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q15.c
index 01d476e6c..01d476e6c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q31.c
index 3a5c523df..3a5c523df 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_f32.c
index fb79e3172..fb79e3172 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_f32.c
index b5f91ea32..b5f91ea32 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q15.c
index c6452fe98..c6452fe98 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q31.c
index 211cdadf3..211cdadf3 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q15.c
index feaa2ae3c..feaa2ae3c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q31.c
index 887cfb385..887cfb385 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_dct4_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_f32.c
index 2043a4e0d..2043a4e0d 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_f32.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_f32.c
index 7a54df6e2..7a54df6e2 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_f32.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_f32.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q15.c
index 50b9d994d..50b9d994d 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q31.c
index 400fe586c..400fe586c 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q31.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q15.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q15.c
index c0c201277..c0c201277 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q15.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q15.c
diff --git a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q31.c b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q31.c
index afcb47041..afcb47041 100644
--- a/apps/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q31.c
+++ b/src/modules/mathlib/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q31.c
diff --git a/apps/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
index 8f39acd9d..8f39acd9d 100644
--- a/apps/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
+++ b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
diff --git a/apps/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
index 181b7e433..181b7e433 100644
--- a/apps/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
+++ b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
diff --git a/apps/mathlib/CMSIS/Include/arm_common_tables.h b/src/modules/mathlib/CMSIS/Include/arm_common_tables.h
index 5fd6ff4af..5fd6ff4af 100644
--- a/apps/mathlib/CMSIS/Include/arm_common_tables.h
+++ b/src/modules/mathlib/CMSIS/Include/arm_common_tables.h
diff --git a/apps/mathlib/CMSIS/Include/arm_math.h b/src/modules/mathlib/CMSIS/Include/arm_math.h
index f224d3eb0..f224d3eb0 100644
--- a/apps/mathlib/CMSIS/Include/arm_math.h
+++ b/src/modules/mathlib/CMSIS/Include/arm_math.h
diff --git a/apps/mathlib/CMSIS/Include/core_cm3.h b/src/modules/mathlib/CMSIS/Include/core_cm3.h
index 733d6be53..733d6be53 100644
--- a/apps/mathlib/CMSIS/Include/core_cm3.h
+++ b/src/modules/mathlib/CMSIS/Include/core_cm3.h
diff --git a/apps/mathlib/CMSIS/Include/core_cm4.h b/src/modules/mathlib/CMSIS/Include/core_cm4.h
index 5f3b7d619..5f3b7d619 100644
--- a/apps/mathlib/CMSIS/Include/core_cm4.h
+++ b/src/modules/mathlib/CMSIS/Include/core_cm4.h
diff --git a/apps/mathlib/CMSIS/Include/core_cm4_simd.h b/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h
index b5140073f..b5140073f 100644
--- a/apps/mathlib/CMSIS/Include/core_cm4_simd.h
+++ b/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h
diff --git a/apps/mathlib/CMSIS/Include/core_cmFunc.h b/src/modules/mathlib/CMSIS/Include/core_cmFunc.h
index adb07b5d3..adb07b5d3 100644
--- a/apps/mathlib/CMSIS/Include/core_cmFunc.h
+++ b/src/modules/mathlib/CMSIS/Include/core_cmFunc.h
diff --git a/apps/mathlib/CMSIS/Include/core_cmInstr.h b/src/modules/mathlib/CMSIS/Include/core_cmInstr.h
index 624c175fd..624c175fd 100644
--- a/apps/mathlib/CMSIS/Include/core_cmInstr.h
+++ b/src/modules/mathlib/CMSIS/Include/core_cmInstr.h
diff --git a/apps/mathlib/CMSIS/Makefile b/src/modules/mathlib/CMSIS/module.mk
index 9e28518bc..5e1abe642 100644
--- a/apps/mathlib/CMSIS/Makefile
+++ b/src/modules/mathlib/CMSIS/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -39,9 +39,10 @@
# Find sources
#
DSPLIB_SRCDIR := $(dir $(lastword $(MAKEFILE_LIST)))
-CSRCS := $(wildcard $(DSPLIB_SRCDIR)/DSP_Lib/Source/*/*.c)
+SRCLIST := $(wildcard $(DSPLIB_SRCDIR)DSP_Lib/Source/*/*.c)
+SRCS := $(patsubst $(DSPLIB_SRCDIR)%,%,$(SRCLIST))
-INCLUDES += $(DSPLIB_SRCDIR)/Include \
+INCLUDE_DIRS += $(DSPLIB_SRCDIR)/Include \
$(DSPLIB_SRCDIR)/Device/ARM/ARMCM4/Include \
$(DSPLIB_SRCDIR)/Device/ARM/ARMCM3/Include
@@ -57,5 +58,3 @@ EXTRADEFINES += -Wno-unsuffixed-float-constants \
# have anything we can use to mark exported symbols.
#
DEFAULT_VISIBILITY = YES
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp
index df0f09b20..c3742e288 100644
--- a/apps/mathlib/math/Dcm.cpp
+++ b/src/modules/mathlib/math/Dcm.cpp
@@ -37,7 +37,7 @@
* math direction cosine matrix
*/
-#include "math/test/test.hpp"
+#include <mathlib/math/test/test.hpp>
#include "Dcm.hpp"
#include "Quaternion.hpp"
diff --git a/apps/mathlib/math/Dcm.hpp b/src/modules/mathlib/math/Dcm.hpp
index 28d840b10..28d840b10 100644
--- a/apps/mathlib/math/Dcm.hpp
+++ b/src/modules/mathlib/math/Dcm.hpp
diff --git a/apps/mathlib/math/EulerAngles.cpp b/src/modules/mathlib/math/EulerAngles.cpp
index 2e96fef4c..e733d23bb 100644
--- a/apps/mathlib/math/EulerAngles.cpp
+++ b/src/modules/mathlib/math/EulerAngles.cpp
@@ -37,7 +37,7 @@
* math vector
*/
-#include "math/test/test.hpp"
+#include "test/test.hpp"
#include "EulerAngles.hpp"
#include "Quaternion.hpp"
diff --git a/apps/mathlib/math/EulerAngles.hpp b/src/modules/mathlib/math/EulerAngles.hpp
index 399eecfa7..399eecfa7 100644
--- a/apps/mathlib/math/EulerAngles.hpp
+++ b/src/modules/mathlib/math/EulerAngles.hpp
diff --git a/apps/mathlib/math/Matrix.cpp b/src/modules/mathlib/math/Matrix.cpp
index 981732370..ebd1aeda3 100644
--- a/apps/mathlib/math/Matrix.cpp
+++ b/src/modules/mathlib/math/Matrix.cpp
@@ -37,7 +37,7 @@
* matrix code
*/
-#include "math/test/test.hpp"
+#include "test/test.hpp"
#include <math.h>
#include "Matrix.hpp"
diff --git a/apps/mathlib/math/Matrix.hpp b/src/modules/mathlib/math/Matrix.hpp
index f19db15ec..f19db15ec 100644
--- a/apps/mathlib/math/Matrix.hpp
+++ b/src/modules/mathlib/math/Matrix.hpp
diff --git a/apps/mathlib/math/Quaternion.cpp b/src/modules/mathlib/math/Quaternion.cpp
index 78481b286..02fec4ca6 100644
--- a/apps/mathlib/math/Quaternion.cpp
+++ b/src/modules/mathlib/math/Quaternion.cpp
@@ -37,7 +37,7 @@
* math vector
*/
-#include "math/test/test.hpp"
+#include "test/test.hpp"
#include "Quaternion.hpp"
diff --git a/apps/mathlib/math/Quaternion.hpp b/src/modules/mathlib/math/Quaternion.hpp
index 4b4e959d8..4b4e959d8 100644
--- a/apps/mathlib/math/Quaternion.hpp
+++ b/src/modules/mathlib/math/Quaternion.hpp
diff --git a/apps/mathlib/math/Vector.cpp b/src/modules/mathlib/math/Vector.cpp
index d58e719db..35158a396 100644
--- a/apps/mathlib/math/Vector.cpp
+++ b/src/modules/mathlib/math/Vector.cpp
@@ -37,7 +37,7 @@
* math vector
*/
-#include "math/test/test.hpp"
+#include "test/test.hpp"
#include "Vector.hpp"
diff --git a/apps/mathlib/math/Vector.hpp b/src/modules/mathlib/math/Vector.hpp
index 73de793d5..73de793d5 100644
--- a/apps/mathlib/math/Vector.hpp
+++ b/src/modules/mathlib/math/Vector.hpp
diff --git a/apps/mathlib/math/Vector3.cpp b/src/modules/mathlib/math/Vector3.cpp
index 9c57506da..61fcc442f 100644
--- a/apps/mathlib/math/Vector3.cpp
+++ b/src/modules/mathlib/math/Vector3.cpp
@@ -37,7 +37,7 @@
* math vector
*/
-#include "math/test/test.hpp"
+#include "test/test.hpp"
#include "Vector3.hpp"
diff --git a/apps/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp
index 8c36ac134..8c36ac134 100644
--- a/apps/mathlib/math/Vector3.hpp
+++ b/src/modules/mathlib/math/Vector3.hpp
diff --git a/apps/mathlib/math/arm/Matrix.cpp b/src/modules/mathlib/math/arm/Matrix.cpp
index 21661622a..21661622a 100644
--- a/apps/mathlib/math/arm/Matrix.cpp
+++ b/src/modules/mathlib/math/arm/Matrix.cpp
diff --git a/apps/mathlib/math/arm/Matrix.hpp b/src/modules/mathlib/math/arm/Matrix.hpp
index 715fd3a5e..715fd3a5e 100644
--- a/apps/mathlib/math/arm/Matrix.hpp
+++ b/src/modules/mathlib/math/arm/Matrix.hpp
diff --git a/apps/mathlib/math/arm/Vector.cpp b/src/modules/mathlib/math/arm/Vector.cpp
index 7ea6496bb..7ea6496bb 100644
--- a/apps/mathlib/math/arm/Vector.cpp
+++ b/src/modules/mathlib/math/arm/Vector.cpp
diff --git a/apps/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp
index 58d51107d..58d51107d 100644
--- a/apps/mathlib/math/arm/Vector.hpp
+++ b/src/modules/mathlib/math/arm/Vector.hpp
diff --git a/apps/mathlib/math/generic/Matrix.cpp b/src/modules/mathlib/math/generic/Matrix.cpp
index 21661622a..21661622a 100644
--- a/apps/mathlib/math/generic/Matrix.cpp
+++ b/src/modules/mathlib/math/generic/Matrix.cpp
diff --git a/apps/mathlib/math/generic/Matrix.hpp b/src/modules/mathlib/math/generic/Matrix.hpp
index 5601a3447..5601a3447 100644
--- a/apps/mathlib/math/generic/Matrix.hpp
+++ b/src/modules/mathlib/math/generic/Matrix.hpp
diff --git a/apps/mathlib/math/generic/Vector.cpp b/src/modules/mathlib/math/generic/Vector.cpp
index 7ea6496bb..7ea6496bb 100644
--- a/apps/mathlib/math/generic/Vector.cpp
+++ b/src/modules/mathlib/math/generic/Vector.cpp
diff --git a/apps/mathlib/math/generic/Vector.hpp b/src/modules/mathlib/math/generic/Vector.hpp
index 1a7363779..1a7363779 100644
--- a/apps/mathlib/math/generic/Vector.hpp
+++ b/src/modules/mathlib/math/generic/Vector.hpp
diff --git a/apps/mathlib/math/nasa_rotation_def.pdf b/src/modules/mathlib/math/nasa_rotation_def.pdf
index eb67a4bfc..eb67a4bfc 100644
--- a/apps/mathlib/math/nasa_rotation_def.pdf
+++ b/src/modules/mathlib/math/nasa_rotation_def.pdf
Binary files differ
diff --git a/apps/mathlib/math/test/test.cpp b/src/modules/mathlib/math/test/test.cpp
index f5db76d70..2fa2f7e7c 100644
--- a/apps/mathlib/math/test/test.cpp
+++ b/src/modules/mathlib/math/test/test.cpp
@@ -39,6 +39,7 @@
#include <stdio.h>
#include <math.h>
+#include <stdlib.h>
#include "test.hpp"
diff --git a/apps/mathlib/math/test/test.hpp b/src/modules/mathlib/math/test/test.hpp
index 841c42144..2027bb827 100644
--- a/apps/mathlib/math/test/test.hpp
+++ b/src/modules/mathlib/math/test/test.hpp
@@ -39,9 +39,9 @@
#pragma once
-#include <assert.h>
-#include <time.h>
-#include <stdlib.h>
+//#include <assert.h>
+//#include <time.h>
+//#include <stdlib.h>
bool equal(float a, float b, float eps = 1e-5);
void float2SigExp(
diff --git a/apps/mathlib/math/test_math.sce b/src/modules/mathlib/math/test_math.sce
index c3fba4729..c3fba4729 100644
--- a/apps/mathlib/math/test_math.sce
+++ b/src/modules/mathlib/math/test_math.sce
diff --git a/apps/mathlib/mathlib.h b/src/modules/mathlib/mathlib.h
index b919d53db..b919d53db 100644
--- a/apps/mathlib/mathlib.h
+++ b/src/modules/mathlib/mathlib.h
diff --git a/apps/mathlib/Makefile b/src/modules/mathlib/module.mk
index 7eebd6ae0..bcdb2afe5 100644
--- a/apps/mathlib/Makefile
+++ b/src/modules/mathlib/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -34,7 +34,7 @@
#
# Math library
#
-CXXSRCS = math/test/test.cpp \
+SRCS = math/test/test.cpp \
math/Vector.cpp \
math/Vector3.cpp \
math/EulerAngles.cpp \
@@ -50,13 +50,11 @@ APP_MAKEFILE := $(lastword $(MAKEFILE_LIST))
-include $(TOPDIR)/.config
ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy)
-INCLUDES += math/arm
-CXXSRCS += math/arm/Vector.cpp \
+INCLUDE_DIRS += math/arm
+SRCS += math/arm/Vector.cpp \
math/arm/Matrix.cpp
else
-INCLUDES += math/generic
-CXXSRCS += math/generic/Vector.cpp \
- math/generic/Matrix.cpp
+#INCLUDE_DIRS += math/generic
+#SRCS += math/generic/Vector.cpp \
+# math/generic/Matrix.cpp
endif
-
-include $(APPDIR)/mk/app.mk
diff --git a/apps/mavlink/.gitignore b/src/modules/mavlink/.gitignore
index a02827195..a02827195 100644
--- a/apps/mavlink/.gitignore
+++ b/src/modules/mavlink/.gitignore
diff --git a/apps/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 2643cbf7b..15ba8860d 100644
--- a/apps/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -64,9 +64,9 @@
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
+#include <mavlink/mavlink_log.h>
#include "waypoints.h"
-#include "mavlink_log.h"
#include "orb_topics.h"
#include "missionlib.h"
#include "mavlink_hil.h"
diff --git a/apps/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 270ec1727..0010bb341 100644
--- a/apps/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -51,7 +51,7 @@
#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
-#include "v1.0/mavlink_types.h"
+#include <v1.0/mavlink_types.h>
#include <unistd.h>
diff --git a/apps/mavlink/mavlink_hil.h b/src/modules/mavlink/mavlink_hil.h
index 8c7a5b514..8c7a5b514 100644
--- a/apps/mavlink/mavlink_hil.h
+++ b/src/modules/mavlink/mavlink_hil.h
diff --git a/apps/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c
index ed65fb883..fa974dc0b 100644
--- a/apps/mavlink/mavlink_log.c
+++ b/src/modules/mavlink/mavlink_log.c
@@ -42,7 +42,7 @@
#include <string.h>
#include <stdlib.h>
-#include "mavlink_log.h"
+#include <mavlink/mavlink_log.h>
void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
{
diff --git a/apps/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c
index 819f3441b..819f3441b 100644
--- a/apps/mavlink/mavlink_parameters.c
+++ b/src/modules/mavlink/mavlink_parameters.c
diff --git a/apps/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
index b1e38bcc8..b1e38bcc8 100644
--- a/apps/mavlink/mavlink_parameters.h
+++ b/src/modules/mavlink/mavlink_parameters.h
diff --git a/apps/mavlink/mavlink_receiver.c b/src/modules/mavlink/mavlink_receiver.c
index 4d9cd964d..a42612f9e 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/src/modules/mavlink/mavlink_receiver.c
@@ -64,9 +64,9 @@
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
+#include <mavlink/mavlink_log.h>
#include "waypoints.h"
-#include "mavlink_log.h"
#include "orb_topics.h"
#include "missionlib.h"
#include "mavlink_hil.h"
@@ -308,10 +308,10 @@ handle_message(mavlink_message_t *msg)
uint64_t timestamp = hrt_absolute_time();
- if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) {
+ if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) {
- mavlink_raw_imu_t imu;
- mavlink_msg_raw_imu_decode(msg, &imu);
+ mavlink_highres_imu_t imu;
+ mavlink_msg_highres_imu_decode(msg, &imu);
/* packet counter */
static uint16_t hil_counter = 0;
@@ -319,27 +319,27 @@ handle_message(mavlink_message_t *msg)
static uint64_t old_timestamp = 0;
/* sensors general */
- hil_sensors.timestamp = imu.time_usec;
+ hil_sensors.timestamp = hrt_absolute_time();
/* hil gyro */
static const float mrad2rad = 1.0e-3f;
hil_sensors.gyro_counter = hil_counter;
- hil_sensors.gyro_raw[0] = imu.xgyro;
- hil_sensors.gyro_raw[1] = imu.ygyro;
- hil_sensors.gyro_raw[2] = imu.zgyro;
- hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad;
- hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad;
- hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad;
+ hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
+ hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
+ hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
+ hil_sensors.gyro_rad_s[0] = imu.xgyro;
+ hil_sensors.gyro_rad_s[1] = imu.ygyro;
+ hil_sensors.gyro_rad_s[2] = imu.zgyro;
/* accelerometer */
hil_sensors.accelerometer_counter = hil_counter;
static const float mg2ms2 = 9.8f / 1000.0f;
- hil_sensors.accelerometer_raw[0] = imu.xacc;
- hil_sensors.accelerometer_raw[1] = imu.yacc;
- hil_sensors.accelerometer_raw[2] = imu.zacc;
- hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc;
- hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc;
- hil_sensors.accelerometer_m_s2[2] = mg2ms2 * imu.zacc;
+ hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
+ hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
+ hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
+ hil_sensors.accelerometer_m_s2[0] = imu.xacc;
+ hil_sensors.accelerometer_m_s2[1] = imu.yacc;
+ hil_sensors.accelerometer_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_mode = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
@@ -351,22 +351,31 @@ handle_message(mavlink_message_t *msg)
/* magnetometer */
float mga2ga = 1.0e-3f;
hil_sensors.magnetometer_counter = hil_counter;
- hil_sensors.magnetometer_raw[0] = imu.xmag;
- hil_sensors.magnetometer_raw[1] = imu.ymag;
- hil_sensors.magnetometer_raw[2] = imu.zmag;
- hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga;
- hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga;
- hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga;
+ hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
+ hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
+ hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
+ hil_sensors.magnetometer_ga[0] = imu.xmag;
+ hil_sensors.magnetometer_ga[1] = imu.ymag;
+ hil_sensors.magnetometer_ga[2] = imu.zmag;
hil_sensors.magnetometer_range_ga = 32.7f; // int16
hil_sensors.magnetometer_mode = 0; // TODO what is this
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
+ /* baro */
+ hil_sensors.baro_pres_mbar = imu.abs_pressure;
+ hil_sensors.baro_alt_meter = imu.pressure_alt;
+ hil_sensors.baro_temp_celcius = imu.temperature;
+
+ hil_sensors.gyro_counter = hil_counter;
+ hil_sensors.magnetometer_counter = hil_counter;
+ hil_sensors.accelerometer_counter = hil_counter;
+
/* publish */
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
// increment counters
- hil_counter += 1 ;
- hil_frames += 1 ;
+ hil_counter++;
+ hil_frames++;
// output
if ((timestamp - old_timestamp) > 10000000) {
@@ -388,21 +397,27 @@ handle_message(mavlink_message_t *msg)
/* gps */
hil_gps.timestamp_position = gps.time_usec;
-// hil_gps.counter = hil_counter++;
hil_gps.time_gps_usec = gps.time_usec;
hil_gps.lat = gps.lat;
hil_gps.lon = gps.lon;
hil_gps.alt = gps.alt;
-// hil_gps.counter_pos_valid = hil_counter++;
hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
- hil_gps.s_variance_m_s = 100; // XXX 100 m/s variance?
- hil_gps.p_variance_m = 100; // XXX 100 m variance?
+ hil_gps.s_variance_m_s = 5.0f;
+ hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
- hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
- hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
+
+ /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */
+ float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
+ /* go back to -PI..PI */
+ if (heading_rad > M_PI_F)
+ heading_rad -= 2.0f * M_PI_F;
+ hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad);
+ hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad);
hil_gps.vel_d_m_s = 0.0f;
- hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad
+ hil_gps.vel_ned_valid = true;
+ /* COG (course over ground) is speced as -PI..+PI */
+ hil_gps.cog_rad = heading_rad;
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible;
@@ -421,51 +436,6 @@ handle_message(mavlink_message_t *msg)
}
}
- if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) {
-
- mavlink_raw_pressure_t press;
- mavlink_msg_raw_pressure_decode(msg, &press);
-
- /* packet counter */
- static uint16_t hil_counter = 0;
- static uint16_t hil_frames = 0;
- static uint64_t old_timestamp = 0;
-
- /* sensors general */
- hil_sensors.timestamp = press.time_usec;
-
- /* baro */
- /* TODO, set ground_press/ temp during calib */
- static const float ground_press = 1013.25f; // mbar
- static const float ground_tempC = 21.0f;
- static const float ground_alt = 0.0f;
- static const float T0 = 273.15;
- static const float R = 287.05f;
- static const float g = 9.806f;
-
- float tempC = press.temperature / 100.0f;
- float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f;
- float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs);
- hil_sensors.baro_counter = hil_counter;
- hil_sensors.baro_pres_mbar = press.press_abs;
- hil_sensors.baro_alt_meter = h;
- hil_sensors.baro_temp_celcius = tempC;
-
- /* publish */
- orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
-
- // increment counters
- hil_counter += 1 ;
- hil_frames += 1 ;
-
- // output
- if ((timestamp - old_timestamp) > 10000000) {
- printf("receiving hil pressure at %d hz\n", hil_frames/10);
- old_timestamp = timestamp;
- hil_frames = 0;
- }
- }
-
if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
mavlink_hil_state_t hil_state;
diff --git a/apps/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index 8064febc4..d369e05ff 100644
--- a/apps/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -62,9 +62,9 @@
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
+#include <mavlink/mavlink_log.h>
#include "waypoints.h"
-#include "mavlink_log.h"
#include "orb_topics.h"
#include "missionlib.h"
#include "mavlink_hil.h"
diff --git a/apps/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h
index c2ca735b3..c2ca735b3 100644
--- a/apps/mavlink/missionlib.h
+++ b/src/modules/mavlink/missionlib.h
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
new file mode 100644
index 000000000..cbf08aeb2
--- /dev/null
+++ b/src/modules/mavlink/module.mk
@@ -0,0 +1,47 @@
+############################################################################
+#
+# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# MAVLink protocol to uORB interface process
+#
+
+MODULE_COMMAND = mavlink
+SRCS += mavlink.c \
+ missionlib.c \
+ mavlink_parameters.c \
+ mavlink_log.c \
+ mavlink_receiver.c \
+ orb_listener.c \
+ waypoints.c
+
+INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
diff --git a/apps/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index d9b735380..d84406e7a 100644
--- a/apps/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -56,8 +56,9 @@
#include <stdlib.h>
#include <poll.h>
+#include <mavlink/mavlink_log.h>
+
#include "waypoints.h"
-#include "mavlink_log.h"
#include "orb_topics.h"
#include "missionlib.h"
#include "mavlink_hil.h"
@@ -229,6 +230,13 @@ l_vehicle_gps_position(const struct listener *l)
/* copy gps data into local buffer */
orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
+ /* GPS COG is 0..2PI in degrees * 1e2 */
+ float cog_deg = gps.cog_rad;
+ if (cog_deg > M_PI_F)
+ cog_deg -= 2.0f * M_PI_F;
+ cog_deg *= M_RAD_TO_DEG_F;
+
+
/* GPS position */
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
gps.timestamp_position,
@@ -236,13 +244,14 @@ l_vehicle_gps_position(const struct listener *l)
gps.lat,
gps.lon,
gps.alt,
- (uint16_t)(gps.eph_m * 1e2f), // from m to cm
- (uint16_t)(gps.epv_m * 1e2f), // from m to cm
- (uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s
- (uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100
+ gps.eph_m * 1e2f, // from m to cm
+ gps.epv_m * 1e2f, // from m to cm
+ gps.vel_m_s * 1e2f, // from m/s to cm/s
+ cog_deg * 1e2f, // from rad to deg * 100
gps.satellites_visible);
- if (gps.satellite_info_available && (gps_counter % 4 == 0)) {
+ /* update SAT info every 10 seconds */
+ if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
mavlink_msg_gps_status_send(MAVLINK_COMM_0,
gps.satellites_visible,
gps.satellite_prn,
diff --git a/apps/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index d61cd43dc..d61cd43dc 100644
--- a/apps/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
diff --git a/apps/mavlink/util.h b/src/modules/mavlink/util.h
index a4ff06a88..a4ff06a88 100644
--- a/apps/mavlink/util.h
+++ b/src/modules/mavlink/util.h
diff --git a/apps/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index a131b143b..a131b143b 100644
--- a/apps/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
diff --git a/apps/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index c32ab32e5..c32ab32e5 100644
--- a/apps/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
diff --git a/apps/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
index 408a850d8..408a850d8 100644
--- a/apps/mavlink_onboard/mavlink.c
+++ b/src/modules/mavlink_onboard/mavlink.c
diff --git a/apps/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h
index bf7c5354c..3ad3bb617 100644
--- a/apps/mavlink_onboard/mavlink_bridge_header.h
+++ b/src/modules/mavlink_onboard/mavlink_bridge_header.h
@@ -51,7 +51,7 @@
#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
-#include "v1.0/mavlink_types.h"
+#include <v1.0/mavlink_types.h>
#include <unistd.h>
diff --git a/apps/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c
index 2d406fb9f..2d406fb9f 100644
--- a/apps/mavlink_onboard/mavlink_receiver.c
+++ b/src/modules/mavlink_onboard/mavlink_receiver.c
diff --git a/src/modules/mavlink_onboard/module.mk b/src/modules/mavlink_onboard/module.mk
new file mode 100644
index 000000000..a7a4980fa
--- /dev/null
+++ b/src/modules/mavlink_onboard/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# MAVLink protocol to uORB interface process (XXX hack for onboard use)
+#
+
+MODULE_COMMAND = mavlink_onboard
+SRCS = mavlink.c \
+ mavlink_receiver.c
+
+INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
diff --git a/apps/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
index f18f56243..f18f56243 100644
--- a/apps/mavlink_onboard/orb_topics.h
+++ b/src/modules/mavlink_onboard/orb_topics.h
diff --git a/apps/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h
index 38a4db372..38a4db372 100644
--- a/apps/mavlink_onboard/util.h
+++ b/src/modules/mavlink_onboard/util.h
diff --git a/src/modules/multirotor_att_control/module.mk b/src/modules/multirotor_att_control/module.mk
new file mode 100755
index 000000000..7569e1c7e
--- /dev/null
+++ b/src/modules/multirotor_att_control/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build multirotor attitude controller
+#
+
+MODULE_COMMAND = multirotor_att_control
+
+SRCS = multirotor_att_control_main.c \
+ multirotor_attitude_control.c \
+ multirotor_rate_control.c
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 3329c5c48..3329c5c48 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 76dbb36d3..76dbb36d3 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h
index 2cf83e443..2cf83e443 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.h
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index deba1ac03..deba1ac03 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h
index 03dec317a..03dec317a 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.h
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.h
diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk
new file mode 100644
index 000000000..d04847745
--- /dev/null
+++ b/src/modules/multirotor_pos_control/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build multirotor position control
+#
+
+MODULE_COMMAND = multirotor_pos_control
+
+SRCS = multirotor_pos_control.c \
+ multirotor_pos_control_params.c
diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 7b8d83aa8..7b8d83aa8 100644
--- a/apps/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index 6b73dc405..6b73dc405 100644
--- a/apps/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
index 274f6c22a..274f6c22a 100644
--- a/apps/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
diff --git a/apps/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c
index 9c53a5bf6..9c53a5bf6 100644
--- a/apps/multirotor_pos_control/position_control.c
+++ b/src/modules/multirotor_pos_control/position_control.c
diff --git a/apps/multirotor_pos_control/position_control.h b/src/modules/multirotor_pos_control/position_control.h
index 2144ebc34..2144ebc34 100644
--- a/apps/multirotor_pos_control/position_control.h
+++ b/src/modules/multirotor_pos_control/position_control.h
diff --git a/apps/position_estimator/Makefile b/src/modules/position_estimator/module.mk
index cc5072152..f64095d9d 100644
--- a/apps/position_estimator/Makefile
+++ b/src/modules/position_estimator/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,10 +35,10 @@
# Makefile to build the position estimator
#
-APPNAME = position_estimator
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 4096
+MODULE_COMMAND = position_estimator
-CSRCS = position_estimator_main.c
+# XXX this should be converted to a deamon, its a pretty bad example app
+MODULE_PRIORITY = SCHED_PRIORITY_DEFAULT
+MODULE_STACKSIZE = 4096
-include $(APPDIR)/mk/app.mk
+SRCS = position_estimator_main.c
diff --git a/apps/position_estimator/position_estimator_main.c b/src/modules/position_estimator/position_estimator_main.c
index e84945299..e84945299 100644
--- a/apps/position_estimator/position_estimator_main.c
+++ b/src/modules/position_estimator/position_estimator_main.c
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c
new file mode 100755
index 000000000..977565b8e
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c
@@ -0,0 +1,58 @@
+/*
+ * kalman_dlqe1.c
+ *
+ * Code generation for function 'kalman_dlqe1'
+ *
+ * C source code generated on: Wed Feb 13 20:34:32 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe1.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3],
+ const real32_T x_aposteriori_k[3], real32_T z, real32_T
+ x_aposteriori[3])
+{
+ printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z);
+ printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2]));
+ real32_T y;
+ int32_T i0;
+ real32_T b_y[3];
+ int32_T i1;
+ real32_T f0;
+ y = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ b_y[i0] = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ b_y[i0] += C[i1] * A[i1 + 3 * i0];
+ }
+
+ y += b_y[i0] * x_aposteriori_k[i0];
+ }
+
+ y = z - y;
+ for (i0 = 0; i0 < 3; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
+ }
+
+ x_aposteriori[i0] = f0 + K[i0] * y;
+ }
+ //printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2]));
+}
+
+/* End of code generation (kalman_dlqe1.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h
new file mode 100755
index 000000000..2f5330563
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h
@@ -0,0 +1,30 @@
+/*
+ * kalman_dlqe1.h
+ *
+ * Code generation for function 'kalman_dlqe1'
+ *
+ * C source code generated on: Wed Feb 13 20:34:32 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE1_H__
+#define __KALMAN_DLQE1_H__
+/* Include files */
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "kalman_dlqe1_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
+#endif
+/* End of code generation (kalman_dlqe1.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c
new file mode 100755
index 000000000..6627f76e9
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c
@@ -0,0 +1,31 @@
+/*
+ * kalman_dlqe1_initialize.c
+ *
+ * Code generation for function 'kalman_dlqe1_initialize'
+ *
+ * C source code generated on: Wed Feb 13 20:34:31 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe1.h"
+#include "kalman_dlqe1_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe1_initialize(void)
+{
+ rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (kalman_dlqe1_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h
new file mode 100755
index 000000000..a77eb5712
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h
@@ -0,0 +1,30 @@
+/*
+ * kalman_dlqe1_initialize.h
+ *
+ * Code generation for function 'kalman_dlqe1_initialize'
+ *
+ * C source code generated on: Wed Feb 13 20:34:31 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE1_INITIALIZE_H__
+#define __KALMAN_DLQE1_INITIALIZE_H__
+/* Include files */
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "kalman_dlqe1_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe1_initialize(void);
+#endif
+/* End of code generation (kalman_dlqe1_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c
new file mode 100755
index 000000000..a65536f79
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c
@@ -0,0 +1,31 @@
+/*
+ * kalman_dlqe1_terminate.c
+ *
+ * Code generation for function 'kalman_dlqe1_terminate'
+ *
+ * C source code generated on: Wed Feb 13 20:34:31 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe1.h"
+#include "kalman_dlqe1_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe1_terminate(void)
+{
+ /* (no terminate code required) */
+}
+
+/* End of code generation (kalman_dlqe1_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h
new file mode 100755
index 000000000..100c7f76c
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h
@@ -0,0 +1,30 @@
+/*
+ * kalman_dlqe1_terminate.h
+ *
+ * Code generation for function 'kalman_dlqe1_terminate'
+ *
+ * C source code generated on: Wed Feb 13 20:34:32 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE1_TERMINATE_H__
+#define __KALMAN_DLQE1_TERMINATE_H__
+/* Include files */
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "kalman_dlqe1_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe1_terminate(void);
+#endif
+/* End of code generation (kalman_dlqe1_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h
new file mode 100755
index 000000000..d4b2c2d61
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h
@@ -0,0 +1,16 @@
+/*
+ * kalman_dlqe1_types.h
+ *
+ * Code generation for function 'kalman_dlqe1'
+ *
+ * C source code generated on: Wed Feb 13 20:34:31 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE1_TYPES_H__
+#define __KALMAN_DLQE1_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (kalman_dlqe1_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c
new file mode 100755
index 000000000..11b999064
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c
@@ -0,0 +1,119 @@
+/*
+ * kalman_dlqe2.c
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe2.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+static real32_T rt_powf_snf(real32_T u0, real32_T u1);
+
+/* Function Definitions */
+static real32_T rt_powf_snf(real32_T u0, real32_T u1)
+{
+ real32_T y;
+ real32_T f1;
+ real32_T f2;
+ if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
+ y = ((real32_T)rtNaN);
+ } else {
+ f1 = (real32_T)fabs(u0);
+ f2 = (real32_T)fabs(u1);
+ if (rtIsInfF(u1)) {
+ if (f1 == 1.0F) {
+ y = ((real32_T)rtNaN);
+ } else if (f1 > 1.0F) {
+ if (u1 > 0.0F) {
+ y = ((real32_T)rtInf);
+ } else {
+ y = 0.0F;
+ }
+ } else if (u1 > 0.0F) {
+ y = 0.0F;
+ } else {
+ y = ((real32_T)rtInf);
+ }
+ } else if (f2 == 0.0F) {
+ y = 1.0F;
+ } else if (f2 == 1.0F) {
+ if (u1 > 0.0F) {
+ y = u0;
+ } else {
+ y = 1.0F / u0;
+ }
+ } else if (u1 == 2.0F) {
+ y = u0 * u0;
+ } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
+ y = (real32_T)sqrt(u0);
+ } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
+ y = ((real32_T)rtNaN);
+ } else {
+ y = (real32_T)pow(u0, u1);
+ }
+ }
+
+ return y;
+}
+
+void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
+ real32_T x_aposteriori_k[3], real32_T z, real32_T
+ x_aposteriori[3])
+{
+ //printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3));
+ //printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3));
+ real32_T A[9];
+ real32_T y;
+ int32_T i0;
+ static const int8_T iv0[3] = { 0, 0, 1 };
+
+ real32_T b_k1[3];
+ int32_T i1;
+ static const int8_T iv1[3] = { 1, 0, 0 };
+
+ real32_T f0;
+ A[0] = 1.0F;
+ A[3] = dt;
+ A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
+ A[1] = 0.0F;
+ A[4] = 1.0F;
+ A[7] = dt;
+ y = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ A[2 + 3 * i0] = (real32_T)iv0[i0];
+ b_k1[i0] = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
+ }
+
+ y += b_k1[i0] * x_aposteriori_k[i0];
+ }
+
+ y = z - y;
+ b_k1[0] = k1;
+ b_k1[1] = k2;
+ b_k1[2] = k3;
+ for (i0 = 0; i0 < 3; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
+ }
+
+ x_aposteriori[i0] = f0 + b_k1[i0] * y;
+ }
+}
+
+/* End of code generation (kalman_dlqe2.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h
new file mode 100755
index 000000000..30170ae22
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h
@@ -0,0 +1,32 @@
+/*
+ * kalman_dlqe2.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE2_H__
+#define __KALMAN_DLQE2_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "kalman_dlqe2_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
+#endif
+/* End of code generation (kalman_dlqe2.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c
new file mode 100755
index 000000000..de5a1d8aa
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c
@@ -0,0 +1,31 @@
+/*
+ * kalman_dlqe2_initialize.c
+ *
+ * Code generation for function 'kalman_dlqe2_initialize'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe2.h"
+#include "kalman_dlqe2_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe2_initialize(void)
+{
+ rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (kalman_dlqe2_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h
new file mode 100755
index 000000000..3d507ff31
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h
@@ -0,0 +1,32 @@
+/*
+ * kalman_dlqe2_initialize.h
+ *
+ * Code generation for function 'kalman_dlqe2_initialize'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE2_INITIALIZE_H__
+#define __KALMAN_DLQE2_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "kalman_dlqe2_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe2_initialize(void);
+#endif
+/* End of code generation (kalman_dlqe2_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c
new file mode 100755
index 000000000..0757c878c
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c
@@ -0,0 +1,31 @@
+/*
+ * kalman_dlqe2_terminate.c
+ *
+ * Code generation for function 'kalman_dlqe2_terminate'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe2.h"
+#include "kalman_dlqe2_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe2_terminate(void)
+{
+ /* (no terminate code required) */
+}
+
+/* End of code generation (kalman_dlqe2_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h
new file mode 100755
index 000000000..23995020b
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h
@@ -0,0 +1,32 @@
+/*
+ * kalman_dlqe2_terminate.h
+ *
+ * Code generation for function 'kalman_dlqe2_terminate'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE2_TERMINATE_H__
+#define __KALMAN_DLQE2_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "kalman_dlqe2_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe2_terminate(void);
+#endif
+/* End of code generation (kalman_dlqe2_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h
new file mode 100755
index 000000000..f7a04d908
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h
@@ -0,0 +1,16 @@
+/*
+ * kalman_dlqe2_types.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:28 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE2_TYPES_H__
+#define __KALMAN_DLQE2_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (kalman_dlqe2_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c
new file mode 100755
index 000000000..9efe2ea7a
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c
@@ -0,0 +1,137 @@
+/*
+ * kalman_dlqe3.c
+ *
+ * Code generation for function 'kalman_dlqe3'
+ *
+ * C source code generated on: Tue Feb 19 15:26:31 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe3.h"
+#include "randn.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+static real32_T rt_powf_snf(real32_T u0, real32_T u1);
+
+/* Function Definitions */
+static real32_T rt_powf_snf(real32_T u0, real32_T u1)
+{
+ real32_T y;
+ real32_T f1;
+ real32_T f2;
+ if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
+ y = ((real32_T)rtNaN);
+ } else {
+ f1 = (real32_T)fabs(u0);
+ f2 = (real32_T)fabs(u1);
+ if (rtIsInfF(u1)) {
+ if (f1 == 1.0F) {
+ y = ((real32_T)rtNaN);
+ } else if (f1 > 1.0F) {
+ if (u1 > 0.0F) {
+ y = ((real32_T)rtInf);
+ } else {
+ y = 0.0F;
+ }
+ } else if (u1 > 0.0F) {
+ y = 0.0F;
+ } else {
+ y = ((real32_T)rtInf);
+ }
+ } else if (f2 == 0.0F) {
+ y = 1.0F;
+ } else if (f2 == 1.0F) {
+ if (u1 > 0.0F) {
+ y = u0;
+ } else {
+ y = 1.0F / u0;
+ }
+ } else if (u1 == 2.0F) {
+ y = u0 * u0;
+ } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
+ y = (real32_T)sqrt(u0);
+ } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
+ y = ((real32_T)rtNaN);
+ } else {
+ y = (real32_T)pow(u0, u1);
+ }
+ }
+
+ return y;
+}
+
+void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
+ real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate,
+ real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3])
+{
+ real32_T A[9];
+ int32_T i0;
+ static const int8_T iv0[3] = { 0, 0, 1 };
+
+ real_T b;
+ real32_T y;
+ real32_T b_y[3];
+ int32_T i1;
+ static const int8_T iv1[3] = { 1, 0, 0 };
+
+ real32_T b_k1[3];
+ real32_T f0;
+ A[0] = 1.0F;
+ A[3] = dt;
+ A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
+ A[1] = 0.0F;
+ A[4] = 1.0F;
+ A[7] = dt;
+ for (i0 = 0; i0 < 3; i0++) {
+ A[2 + 3 * i0] = (real32_T)iv0[i0];
+ }
+
+ if (addNoise == 1.0F) {
+ b = randn();
+ z += sigma * (real32_T)b;
+ }
+
+ if (posUpdate != 0.0F) {
+ y = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ b_y[i0] = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
+ }
+
+ y += b_y[i0] * x_aposteriori_k[i0];
+ }
+
+ y = z - y;
+ b_k1[0] = k1;
+ b_k1[1] = k2;
+ b_k1[2] = k3;
+ for (i0 = 0; i0 < 3; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
+ }
+
+ x_aposteriori[i0] = f0 + b_k1[i0] * y;
+ }
+ } else {
+ for (i0 = 0; i0 < 3; i0++) {
+ x_aposteriori[i0] = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1];
+ }
+ }
+ }
+}
+
+/* End of code generation (kalman_dlqe3.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h
new file mode 100755
index 000000000..9bbffbbb3
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h
@@ -0,0 +1,33 @@
+/*
+ * kalman_dlqe3.h
+ *
+ * Code generation for function 'kalman_dlqe3'
+ *
+ * C source code generated on: Tue Feb 19 15:26:32 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE3_H__
+#define __KALMAN_DLQE3_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "kalman_dlqe3_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]);
+#endif
+/* End of code generation (kalman_dlqe3.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c
new file mode 100755
index 000000000..8f2275c13
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c
@@ -0,0 +1,32 @@
+/*
+ * kalman_dlqe3_data.c
+ *
+ * Code generation for function 'kalman_dlqe3_data'
+ *
+ * C source code generated on: Tue Feb 19 15:26:31 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe3.h"
+#include "kalman_dlqe3_data.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+uint32_T method;
+uint32_T state[2];
+uint32_T b_method;
+uint32_T b_state;
+uint32_T c_state[2];
+boolean_T state_not_empty;
+
+/* Function Declarations */
+
+/* Function Definitions */
+/* End of code generation (kalman_dlqe3_data.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h
new file mode 100755
index 000000000..952eb7b89
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h
@@ -0,0 +1,38 @@
+/*
+ * kalman_dlqe3_data.h
+ *
+ * Code generation for function 'kalman_dlqe3_data'
+ *
+ * C source code generated on: Tue Feb 19 15:26:31 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE3_DATA_H__
+#define __KALMAN_DLQE3_DATA_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "kalman_dlqe3_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+extern uint32_T method;
+extern uint32_T state[2];
+extern uint32_T b_method;
+extern uint32_T b_state;
+extern uint32_T c_state[2];
+extern boolean_T state_not_empty;
+
+/* Variable Definitions */
+
+/* Function Declarations */
+#endif
+/* End of code generation (kalman_dlqe3_data.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c
new file mode 100755
index 000000000..b87d604c4
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c
@@ -0,0 +1,47 @@
+/*
+ * kalman_dlqe3_initialize.c
+ *
+ * Code generation for function 'kalman_dlqe3_initialize'
+ *
+ * C source code generated on: Tue Feb 19 15:26:31 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe3.h"
+#include "kalman_dlqe3_initialize.h"
+#include "kalman_dlqe3_data.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe3_initialize(void)
+{
+ int32_T i;
+ static const uint32_T uv0[2] = { 362436069U, 0U };
+
+ rt_InitInfAndNaN(8U);
+ state_not_empty = FALSE;
+ b_state = 1144108930U;
+ b_method = 7U;
+ method = 0U;
+ for (i = 0; i < 2; i++) {
+ c_state[i] = 362436069U + 158852560U * (uint32_T)i;
+ state[i] = uv0[i];
+ }
+
+ if (state[1] == 0U) {
+ state[1] = 521288629U;
+ }
+}
+
+/* End of code generation (kalman_dlqe3_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h
new file mode 100755
index 000000000..9dee90f9e
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h
@@ -0,0 +1,33 @@
+/*
+ * kalman_dlqe3_initialize.h
+ *
+ * Code generation for function 'kalman_dlqe3_initialize'
+ *
+ * C source code generated on: Tue Feb 19 15:26:31 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE3_INITIALIZE_H__
+#define __KALMAN_DLQE3_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "kalman_dlqe3_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe3_initialize(void);
+#endif
+/* End of code generation (kalman_dlqe3_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c
new file mode 100755
index 000000000..b00858205
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c
@@ -0,0 +1,31 @@
+/*
+ * kalman_dlqe3_terminate.c
+ *
+ * Code generation for function 'kalman_dlqe3_terminate'
+ *
+ * C source code generated on: Tue Feb 19 15:26:31 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe3.h"
+#include "kalman_dlqe3_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void kalman_dlqe3_terminate(void)
+{
+ /* (no terminate code required) */
+}
+
+/* End of code generation (kalman_dlqe3_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h
new file mode 100755
index 000000000..69cc85c76
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h
@@ -0,0 +1,33 @@
+/*
+ * kalman_dlqe3_terminate.h
+ *
+ * Code generation for function 'kalman_dlqe3_terminate'
+ *
+ * C source code generated on: Tue Feb 19 15:26:31 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE3_TERMINATE_H__
+#define __KALMAN_DLQE3_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "kalman_dlqe3_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void kalman_dlqe3_terminate(void);
+#endif
+/* End of code generation (kalman_dlqe3_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h
new file mode 100755
index 000000000..f36ea4557
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h
@@ -0,0 +1,16 @@
+/*
+ * kalman_dlqe3_types.h
+ *
+ * Code generation for function 'kalman_dlqe3'
+ *
+ * C source code generated on: Tue Feb 19 15:26:30 2013
+ *
+ */
+
+#ifndef __KALMAN_DLQE3_TYPES_H__
+#define __KALMAN_DLQE3_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (kalman_dlqe3_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c
new file mode 100755
index 000000000..5139848bc
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c
@@ -0,0 +1,136 @@
+/*
+ * positionKalmanFilter1D.c
+ *
+ * Code generation for function 'positionKalmanFilter1D'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const
+ real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T
+ P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T
+ Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3],
+ real32_T P_aposteriori[9])
+{
+ int32_T i0;
+ real32_T f0;
+ int32_T k;
+ real32_T b_A[9];
+ int32_T i1;
+ real32_T P_apriori[9];
+ real32_T y;
+ real32_T K[3];
+ real32_T S;
+ int8_T I[9];
+
+ /* prediction */
+ for (i0 = 0; i0 < 3; i0++) {
+ f0 = 0.0F;
+ for (k = 0; k < 3; k++) {
+ f0 += A[i0 + 3 * k] * x_aposteriori_k[k];
+ }
+
+ x_aposteriori[i0] = f0 + B[i0] * u;
+ }
+
+ for (i0 = 0; i0 < 3; i0++) {
+ for (k = 0; k < 3; k++) {
+ b_A[i0 + 3 * k] = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k];
+ }
+ }
+ }
+
+ for (i0 = 0; i0 < 3; i0++) {
+ for (k = 0; k < 3; k++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1];
+ }
+
+ P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k];
+ }
+ }
+
+ if ((real32_T)fabs(u) < thresh) {
+ x_aposteriori[1] *= decay;
+ }
+
+ /* update */
+ if (gps_update == 1) {
+ y = 0.0F;
+ for (k = 0; k < 3; k++) {
+ y += C[k] * x_aposteriori[k];
+ K[k] = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ K[k] += C[i0] * P_apriori[i0 + 3 * k];
+ }
+ }
+
+ y = z - y;
+ S = 0.0F;
+ for (k = 0; k < 3; k++) {
+ S += K[k] * C[k];
+ }
+
+ S += R;
+ for (i0 = 0; i0 < 3; i0++) {
+ f0 = 0.0F;
+ for (k = 0; k < 3; k++) {
+ f0 += P_apriori[i0 + 3 * k] * C[k];
+ }
+
+ K[i0] = f0 / S;
+ }
+
+ for (i0 = 0; i0 < 3; i0++) {
+ x_aposteriori[i0] += K[i0] * y;
+ }
+
+ for (i0 = 0; i0 < 9; i0++) {
+ I[i0] = 0;
+ }
+
+ for (k = 0; k < 3; k++) {
+ I[k + 3 * k] = 1;
+ }
+
+ for (i0 = 0; i0 < 3; i0++) {
+ for (k = 0; k < 3; k++) {
+ b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 3; i0++) {
+ for (k = 0; k < 3; k++) {
+ P_aposteriori[i0 + 3 * k] = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k];
+ }
+ }
+ }
+ } else {
+ for (i0 = 0; i0 < 9; i0++) {
+ P_aposteriori[i0] = P_apriori[i0];
+ }
+ }
+}
+
+/* End of code generation (positionKalmanFilter1D.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h
new file mode 100755
index 000000000..205c8eb4e
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D.h
+ *
+ * Code generation for function 'positionKalmanFilter1D'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_H__
+#define __POSITIONKALMANFILTER1D_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
+#endif
+/* End of code generation (positionKalmanFilter1D.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c
new file mode 100755
index 000000000..4c535618a
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c
@@ -0,0 +1,157 @@
+/*
+ * positionKalmanFilter1D_dT.c
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D_dT.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3],
+ const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update,
+ const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T
+ x_aposteriori[3], real32_T P_aposteriori[9])
+{
+ real32_T A[9];
+ int32_T i;
+ static const int8_T iv0[3] = { 0, 0, 1 };
+
+ real32_T K[3];
+ real32_T f0;
+ int32_T i0;
+ real32_T b_A[9];
+ int32_T i1;
+ real32_T P_apriori[9];
+ static const int8_T iv1[3] = { 1, 0, 0 };
+
+ real32_T fv0[3];
+ real32_T y;
+ static const int8_T iv2[3] = { 1, 0, 0 };
+
+ real32_T S;
+ int8_T I[9];
+
+ /* dynamics */
+ A[0] = 1.0F;
+ A[3] = dT;
+ A[6] = -0.5F * dT * dT;
+ A[1] = 0.0F;
+ A[4] = 1.0F;
+ A[7] = -dT;
+ for (i = 0; i < 3; i++) {
+ A[2 + 3 * i] = (real32_T)iv0[i];
+ }
+
+ /* prediction */
+ K[0] = 0.5F * dT * dT;
+ K[1] = dT;
+ K[2] = 0.0F;
+ for (i = 0; i < 3; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ f0 += A[i + 3 * i0] * x_aposteriori_k[i0];
+ }
+
+ x_aposteriori[i] = f0 + K[i] * u;
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A[i + 3 * i0] = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1];
+ }
+
+ P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0];
+ }
+ }
+
+ if ((real32_T)fabs(u) < thresh) {
+ x_aposteriori[1] *= decay;
+ }
+
+ /* update */
+ if (gps_update == 1) {
+ f0 = 0.0F;
+ for (i = 0; i < 3; i++) {
+ f0 += (real32_T)iv1[i] * x_aposteriori[i];
+ fv0[i] = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i];
+ }
+ }
+
+ y = z - f0;
+ f0 = 0.0F;
+ for (i = 0; i < 3; i++) {
+ f0 += fv0[i] * (real32_T)iv2[i];
+ }
+
+ S = f0 + (real32_T)R;
+ for (i = 0; i < 3; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0];
+ }
+
+ K[i] = f0 / S;
+ }
+
+ for (i = 0; i < 3; i++) {
+ x_aposteriori[i] += K[i] * y;
+ }
+
+ for (i = 0; i < 9; i++) {
+ I[i] = 0;
+ }
+
+ for (i = 0; i < 3; i++) {
+ I[i + 3 * i] = 1;
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ P_aposteriori[i + 3 * i0] = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0];
+ }
+ }
+ }
+ } else {
+ for (i = 0; i < 9; i++) {
+ P_aposteriori[i] = P_apriori[i];
+ }
+ }
+}
+
+/* End of code generation (positionKalmanFilter1D_dT.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h
new file mode 100755
index 000000000..94cbe2ce6
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_dT.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_DT_H__
+#define __POSITIONKALMANFILTER1D_DT_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_dT_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
+#endif
+/* End of code generation (positionKalmanFilter1D_dT.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c
new file mode 100755
index 000000000..aa89f8a9d
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_dT_initialize.c
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT_initialize'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D_dT.h"
+#include "positionKalmanFilter1D_dT_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D_dT_initialize(void)
+{
+ rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (positionKalmanFilter1D_dT_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h
new file mode 100755
index 000000000..8d358a9a3
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_dT_initialize.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT_initialize'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
+#define __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_dT_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D_dT_initialize(void);
+#endif
+/* End of code generation (positionKalmanFilter1D_dT_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c
new file mode 100755
index 000000000..20ed2edbb
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_dT_terminate.c
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT_terminate'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D_dT.h"
+#include "positionKalmanFilter1D_dT_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D_dT_terminate(void)
+{
+ /* (no terminate code required) */
+}
+
+/* End of code generation (positionKalmanFilter1D_dT_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h
new file mode 100755
index 000000000..5eb5807a0
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_dT_terminate.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT_terminate'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
+#define __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_dT_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D_dT_terminate(void);
+#endif
+/* End of code generation (positionKalmanFilter1D_dT_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h
new file mode 100755
index 000000000..43e5f016c
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h
@@ -0,0 +1,16 @@
+/*
+ * positionKalmanFilter1D_dT_types.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_dT'
+ *
+ * C source code generated on: Fri Nov 30 17:37:33 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_DT_TYPES_H__
+#define __POSITIONKALMANFILTER1D_DT_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (positionKalmanFilter1D_dT_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c
new file mode 100755
index 000000000..5bd87c390
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_initialize.c
+ *
+ * Code generation for function 'positionKalmanFilter1D_initialize'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D.h"
+#include "positionKalmanFilter1D_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D_initialize(void)
+{
+ rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (positionKalmanFilter1D_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h
new file mode 100755
index 000000000..44bce472f
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_initialize.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_initialize'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_INITIALIZE_H__
+#define __POSITIONKALMANFILTER1D_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D_initialize(void);
+#endif
+/* End of code generation (positionKalmanFilter1D_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c
new file mode 100755
index 000000000..41e11936f
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_terminate.c
+ *
+ * Code generation for function 'positionKalmanFilter1D_terminate'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "positionKalmanFilter1D.h"
+#include "positionKalmanFilter1D_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void positionKalmanFilter1D_terminate(void)
+{
+ /* (no terminate code required) */
+}
+
+/* End of code generation (positionKalmanFilter1D_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h
new file mode 100755
index 000000000..e84ea01bc
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h
@@ -0,0 +1,31 @@
+/*
+ * positionKalmanFilter1D_terminate.h
+ *
+ * Code generation for function 'positionKalmanFilter1D_terminate'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_TERMINATE_H__
+#define __POSITIONKALMANFILTER1D_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "rtwtypes.h"
+#include "positionKalmanFilter1D_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void positionKalmanFilter1D_terminate(void);
+#endif
+/* End of code generation (positionKalmanFilter1D_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h
new file mode 100755
index 000000000..4b473f56f
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h
@@ -0,0 +1,16 @@
+/*
+ * positionKalmanFilter1D_types.h
+ *
+ * Code generation for function 'positionKalmanFilter1D'
+ *
+ * C source code generated on: Fri Nov 30 14:26:11 2012
+ *
+ */
+
+#ifndef __POSITIONKALMANFILTER1D_TYPES_H__
+#define __POSITIONKALMANFILTER1D_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (positionKalmanFilter1D_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/randn.c b/src/modules/position_estimator_mc/codegen/randn.c
new file mode 100755
index 000000000..51aef7b76
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/randn.c
@@ -0,0 +1,524 @@
+/*
+ * randn.c
+ *
+ * Code generation for function 'randn'
+ *
+ * C source code generated on: Tue Feb 19 15:26:32 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "kalman_dlqe3.h"
+#include "randn.h"
+#include "kalman_dlqe3_data.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+static uint32_T d_state[625];
+
+/* Function Declarations */
+static real_T b_genrandu(uint32_T mt[625]);
+static real_T eml_rand_mt19937ar(uint32_T e_state[625]);
+static real_T eml_rand_shr3cong(uint32_T e_state[2]);
+static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]);
+static void genrandu(uint32_T s, uint32_T *e_state, real_T *r);
+static void twister_state_vector(uint32_T mt[625], real_T seed);
+
+/* Function Definitions */
+static real_T b_genrandu(uint32_T mt[625])
+{
+ real_T r;
+ int32_T exitg1;
+ uint32_T u[2];
+ boolean_T isvalid;
+ int32_T k;
+ boolean_T exitg2;
+
+ /* <LEGAL> This is a uniform (0,1) pseudorandom number generator based on: */
+ /* <LEGAL> */
+ /* <LEGAL> A C-program for MT19937, with initialization improved 2002/1/26. */
+ /* <LEGAL> Coded by Takuji Nishimura and Makoto Matsumoto. */
+ /* <LEGAL> */
+ /* <LEGAL> Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */
+ /* <LEGAL> All rights reserved. */
+ /* <LEGAL> */
+ /* <LEGAL> Redistribution and use in source and binary forms, with or without */
+ /* <LEGAL> modification, are permitted provided that the following conditions */
+ /* <LEGAL> are met: */
+ /* <LEGAL> */
+ /* <LEGAL> 1. Redistributions of source code must retain the above copyright */
+ /* <LEGAL> notice, this list of conditions and the following disclaimer. */
+ /* <LEGAL> */
+ /* <LEGAL> 2. Redistributions in binary form must reproduce the above copyright */
+ /* <LEGAL> notice, this list of conditions and the following disclaimer in the */
+ /* <LEGAL> documentation and/or other materials provided with the distribution. */
+ /* <LEGAL> */
+ /* <LEGAL> 3. The names of its contributors may not be used to endorse or promote */
+ /* <LEGAL> products derived from this software without specific prior written */
+ /* <LEGAL> permission. */
+ /* <LEGAL> */
+ /* <LEGAL> THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */
+ /* <LEGAL> "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */
+ /* <LEGAL> LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */
+ /* <LEGAL> A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR */
+ /* <LEGAL> CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */
+ /* <LEGAL> EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */
+ /* <LEGAL> PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */
+ /* <LEGAL> PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */
+ /* <LEGAL> LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */
+ /* <LEGAL> NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */
+ /* <LEGAL> SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
+ do {
+ exitg1 = 0;
+ genrand_uint32_vector(mt, u);
+ r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)
+ (u[1] >> 6U));
+ if (r == 0.0) {
+ if ((mt[624] >= 1U) && (mt[624] < 625U)) {
+ isvalid = TRUE;
+ } else {
+ isvalid = FALSE;
+ }
+
+ if (isvalid) {
+ isvalid = FALSE;
+ k = 1;
+ exitg2 = FALSE;
+ while ((exitg2 == FALSE) && (k < 625)) {
+ if (mt[k - 1] == 0U) {
+ k++;
+ } else {
+ isvalid = TRUE;
+ exitg2 = TRUE;
+ }
+ }
+ }
+
+ if (!isvalid) {
+ twister_state_vector(mt, 5489.0);
+ }
+ } else {
+ exitg1 = 1;
+ }
+ } while (exitg1 == 0);
+
+ return r;
+}
+
+static real_T eml_rand_mt19937ar(uint32_T e_state[625])
+{
+ real_T r;
+ int32_T exitg1;
+ uint32_T u32[2];
+ int32_T i;
+ static const real_T dv1[257] = { 0.0, 0.215241895984875, 0.286174591792068,
+ 0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787,
+ 0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557,
+ 0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991,
+ 0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419,
+ 0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975,
+ 0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107,
+ 0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997,
+ 0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153,
+ 0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165,
+ 0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959,
+ 0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086,
+ 0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062,
+ 0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548,
+ 1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093,
+ 1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034,
+ 1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357,
+ 1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815,
+ 1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083,
+ 1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944,
+ 1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653,
+ 1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949,
+ 1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382,
+ 1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073,
+ 1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936,
+ 1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328,
+ 1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998,
+ 1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547,
+ 1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872,
+ 1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808,
+ 1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049,
+ 1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761,
+ 1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417,
+ 1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591,
+ 1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609,
+ 1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487,
+ 1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818,
+ 1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719,
+ 1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782,
+ 1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172,
+ 1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452,
+ 1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082,
+ 1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281,
+ 1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127,
+ 1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043,
+ 1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569,
+ 1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366,
+ 1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053,
+ 2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753,
+ 2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983,
+ 2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952,
+ 2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431,
+ 2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416,
+ 2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379,
+ 2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137,
+ 2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729,
+ 2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715,
+ 2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105,
+ 2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184,
+ 2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674,
+ 2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999,
+ 2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341,
+ 2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214,
+ 3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143,
+ 3.65415288536101, 3.91075795952492 };
+
+ real_T u;
+ static const real_T dv2[257] = { 1.0, 0.977101701267673, 0.959879091800108,
+ 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
+ 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
+ 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
+ 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
+ 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
+ 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
+ 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
+ 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
+ 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
+ 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
+ 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
+ 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
+ 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
+ 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
+ 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
+ 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
+ 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
+ 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
+ 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
+ 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
+ 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
+ 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
+ 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
+ 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
+ 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
+ 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
+ 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
+ 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
+ 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
+ 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
+ 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
+ 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
+ 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
+ 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
+ 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
+ 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
+ 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
+ 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
+ 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
+ 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
+ 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
+ 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
+ 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
+ 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
+ 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
+ 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
+ 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
+ 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
+ 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
+ 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
+ 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
+ 0.093365311912691, 0.0911136480663738, 0.0888735920682759,
+ 0.0866451944505581, 0.0844285095703535, 0.082223595813203,
+ 0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
+ 0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
+ 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
+ 0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
+ 0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
+ 0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
+ 0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
+ 0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
+ 0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
+ 0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
+ 0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
+ 0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
+ 0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
+ 0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
+ 0.000477467764609386 };
+
+ real_T x;
+ do {
+ exitg1 = 0;
+ genrand_uint32_vector(e_state, u32);
+ i = (int32_T)((u32[1] >> 24U) + 1U);
+ r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] &
+ 16777215)) * 2.2204460492503131E-16 - 1.0) * dv1[i];
+ if (fabs(r) <= dv1[i - 1]) {
+ exitg1 = 1;
+ } else if (i < 256) {
+ u = b_genrandu(e_state);
+ if (dv2[i] + u * (dv2[i - 1] - dv2[i]) < exp(-0.5 * r * r)) {
+ exitg1 = 1;
+ }
+ } else {
+ do {
+ u = b_genrandu(e_state);
+ x = log(u) * 0.273661237329758;
+ u = b_genrandu(e_state);
+ } while (!(-2.0 * log(u) > x * x));
+
+ if (r < 0.0) {
+ r = x - 3.65415288536101;
+ } else {
+ r = 3.65415288536101 - x;
+ }
+
+ exitg1 = 1;
+ }
+ } while (exitg1 == 0);
+
+ return r;
+}
+
+static real_T eml_rand_shr3cong(uint32_T e_state[2])
+{
+ real_T r;
+ uint32_T icng;
+ uint32_T jsr;
+ uint32_T ui;
+ int32_T j;
+ static const real_T dv0[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427,
+ 0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948,
+ 0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738,
+ 1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008,
+ 1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122,
+ 1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503,
+ 1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831,
+ 1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713,
+ 2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658,
+ 2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 };
+
+ real_T x;
+ real_T y;
+ real_T s;
+ icng = 69069U * e_state[0] + 1234567U;
+ jsr = e_state[1] ^ e_state[1] << 13U;
+ jsr ^= jsr >> 17U;
+ jsr ^= jsr << 5U;
+ ui = icng + jsr;
+ j = (int32_T)(ui & 63U);
+ r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * dv0[j + 1];
+ if (fabs(r) <= dv0[j]) {
+ } else {
+ x = (fabs(r) - dv0[j]) / (dv0[j + 1] - dv0[j]);
+ icng = 69069U * icng + 1234567U;
+ jsr ^= jsr << 13U;
+ jsr ^= jsr >> 17U;
+ jsr ^= jsr << 5U;
+ y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10;
+ s = x + (0.5 + y);
+ if (s > 1.301198) {
+ if (r < 0.0) {
+ r = 0.4878992 * x - 0.4878992;
+ } else {
+ r = 0.4878992 - 0.4878992 * x;
+ }
+ } else if (s <= 0.9689279) {
+ } else {
+ s = 0.4878992 * x;
+ x = 0.4878992 - 0.4878992 * x;
+ if (0.5 + y > 12.67706 - 12.37586 * exp(-0.5 * (0.4878992 - s) * x)) {
+ if (r < 0.0) {
+ r = -(0.4878992 - s);
+ } else {
+ r = 0.4878992 - s;
+ }
+ } else if (exp(-0.5 * dv0[j + 1] * dv0[j + 1]) + (0.5 + y) * 0.01958303 /
+ dv0[j + 1] <= exp(-0.5 * r * r)) {
+ } else {
+ do {
+ icng = 69069U * icng + 1234567U;
+ jsr ^= jsr << 13U;
+ jsr ^= jsr >> 17U;
+ jsr ^= jsr << 5U;
+ x = log(0.5 + (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10) /
+ 2.776994;
+ icng = 69069U * icng + 1234567U;
+ jsr ^= jsr << 13U;
+ jsr ^= jsr >> 17U;
+ jsr ^= jsr << 5U;
+ } while (!(-2.0 * log(0.5 + (real_T)(int32_T)(icng + jsr) *
+ 2.328306436538696E-10) > x * x));
+
+ if (r < 0.0) {
+ r = x - 2.776994;
+ } else {
+ r = 2.776994 - x;
+ }
+ }
+ }
+ }
+
+ e_state[0] = icng;
+ e_state[1] = jsr;
+ return r;
+}
+
+static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2])
+{
+ int32_T i;
+ uint32_T mti;
+ int32_T kk;
+ uint32_T y;
+ uint32_T b_y;
+ uint32_T c_y;
+ uint32_T d_y;
+ for (i = 0; i < 2; i++) {
+ u[i] = 0U;
+ }
+
+ for (i = 0; i < 2; i++) {
+ mti = mt[624] + 1U;
+ if (mti >= 625U) {
+ for (kk = 0; kk < 227; kk++) {
+ y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U);
+ if ((int32_T)(y & 1U) == 0) {
+ b_y = y >> 1U;
+ } else {
+ b_y = y >> 1U ^ 2567483615U;
+ }
+
+ mt[kk] = mt[397 + kk] ^ b_y;
+ }
+
+ for (kk = 0; kk < 396; kk++) {
+ y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U);
+ if ((int32_T)(y & 1U) == 0) {
+ c_y = y >> 1U;
+ } else {
+ c_y = y >> 1U ^ 2567483615U;
+ }
+
+ mt[227 + kk] = mt[kk] ^ c_y;
+ }
+
+ y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U);
+ if ((int32_T)(y & 1U) == 0) {
+ d_y = y >> 1U;
+ } else {
+ d_y = y >> 1U ^ 2567483615U;
+ }
+
+ mt[623] = mt[396] ^ d_y;
+ mti = 1U;
+ }
+
+ y = mt[(int32_T)mti - 1];
+ mt[624] = mti;
+ y ^= y >> 11U;
+ y ^= y << 7U & 2636928640U;
+ y ^= y << 15U & 4022730752U;
+ y ^= y >> 18U;
+ u[i] = y;
+ }
+}
+
+static void genrandu(uint32_T s, uint32_T *e_state, real_T *r)
+{
+ int32_T hi;
+ uint32_T test1;
+ uint32_T test2;
+ hi = (int32_T)(s / 127773U);
+ test1 = 16807U * (s - (uint32_T)hi * 127773U);
+ test2 = 2836U * (uint32_T)hi;
+ if (test1 < test2) {
+ *e_state = (test1 - test2) + 2147483647U;
+ } else {
+ *e_state = test1 - test2;
+ }
+
+ *r = (real_T)*e_state * 4.6566128752457969E-10;
+}
+
+static void twister_state_vector(uint32_T mt[625], real_T seed)
+{
+ uint32_T r;
+ int32_T mti;
+ if (seed < 4.294967296E+9) {
+ if (seed >= 0.0) {
+ r = (uint32_T)seed;
+ } else {
+ r = 0U;
+ }
+ } else if (seed >= 4.294967296E+9) {
+ r = MAX_uint32_T;
+ } else {
+ r = 0U;
+ }
+
+ mt[0] = r;
+ for (mti = 0; mti < 623; mti++) {
+ r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti);
+ mt[1 + mti] = r;
+ }
+
+ mt[624] = 624U;
+}
+
+real_T randn(void)
+{
+ real_T r;
+ uint32_T e_state;
+ real_T t;
+ real_T b_r;
+ uint32_T f_state;
+ if (method == 0U) {
+ if (b_method == 4U) {
+ do {
+ genrandu(b_state, &e_state, &r);
+ genrandu(e_state, &b_state, &t);
+ b_r = 2.0 * r - 1.0;
+ t = 2.0 * t - 1.0;
+ t = t * t + b_r * b_r;
+ } while (!(t <= 1.0));
+
+ r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);
+ } else if (b_method == 5U) {
+ r = eml_rand_shr3cong(c_state);
+ } else {
+ if (!state_not_empty) {
+ memset(&d_state[0], 0, 625U * sizeof(uint32_T));
+ twister_state_vector(d_state, 5489.0);
+ state_not_empty = TRUE;
+ }
+
+ r = eml_rand_mt19937ar(d_state);
+ }
+ } else if (method == 4U) {
+ e_state = state[0];
+ do {
+ genrandu(e_state, &f_state, &r);
+ genrandu(f_state, &e_state, &t);
+ b_r = 2.0 * r - 1.0;
+ t = 2.0 * t - 1.0;
+ t = t * t + b_r * b_r;
+ } while (!(t <= 1.0));
+
+ state[0] = e_state;
+ r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);
+ } else {
+ r = eml_rand_shr3cong(state);
+ }
+
+ return r;
+}
+
+/* End of code generation (randn.c) */
diff --git a/src/modules/position_estimator_mc/codegen/randn.h b/src/modules/position_estimator_mc/codegen/randn.h
new file mode 100755
index 000000000..8a2aa9277
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/randn.h
@@ -0,0 +1,33 @@
+/*
+ * randn.h
+ *
+ * Code generation for function 'randn'
+ *
+ * C source code generated on: Tue Feb 19 15:26:32 2013
+ *
+ */
+
+#ifndef __RANDN_H__
+#define __RANDN_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "kalman_dlqe3_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern real_T randn(void);
+#endif
+/* End of code generation (randn.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.c b/src/modules/position_estimator_mc/codegen/rtGetInf.c
new file mode 100755
index 000000000..c6fa7884e
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/rtGetInf.c
@@ -0,0 +1,139 @@
+/*
+ * rtGetInf.c
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ * MATLAB for code generation function to initialize non-finite, Inf and MinusInf
+ */
+#include "rtGetInf.h"
+#define NumBitsPerChar 8U
+
+/* Function: rtGetInf ==================================================
+ * Abstract:
+ * Initialize rtInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetInf(void)
+{
+ size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+ real_T inf = 0.0;
+ if (bitsPerReal == 32U) {
+ inf = rtGetInfF();
+ } else {
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ union {
+ LittleEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0x7FF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ inf = tmpVal.fltVal;
+ break;
+ }
+
+ case BigEndian:
+ {
+ union {
+ BigEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0x7FF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ inf = tmpVal.fltVal;
+ break;
+ }
+ }
+ }
+
+ return inf;
+}
+
+/* Function: rtGetInfF ==================================================
+ * Abstract:
+ * Initialize rtInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetInfF(void)
+{
+ IEEESingle infF;
+ infF.wordL.wordLuint = 0x7F800000U;
+ return infF.wordL.wordLreal;
+}
+
+/* Function: rtGetMinusInf ==================================================
+ * Abstract:
+ * Initialize rtMinusInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetMinusInf(void)
+{
+ size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+ real_T minf = 0.0;
+ if (bitsPerReal == 32U) {
+ minf = rtGetMinusInfF();
+ } else {
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ union {
+ LittleEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0xFFF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ minf = tmpVal.fltVal;
+ break;
+ }
+
+ case BigEndian:
+ {
+ union {
+ BigEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0xFFF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ minf = tmpVal.fltVal;
+ break;
+ }
+ }
+ }
+
+ return minf;
+}
+
+/* Function: rtGetMinusInfF ==================================================
+ * Abstract:
+ * Initialize rtMinusInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetMinusInfF(void)
+{
+ IEEESingle minfF;
+ minfF.wordL.wordLuint = 0xFF800000U;
+ return minfF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetInf.c) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.h b/src/modules/position_estimator_mc/codegen/rtGetInf.h
new file mode 100755
index 000000000..e7b2a2d1c
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/rtGetInf.h
@@ -0,0 +1,23 @@
+/*
+ * rtGetInf.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+#ifndef __RTGETINF_H__
+#define __RTGETINF_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetInf(void);
+extern real32_T rtGetInfF(void);
+extern real_T rtGetMinusInf(void);
+extern real32_T rtGetMinusInfF(void);
+
+#endif
+/* End of code generation (rtGetInf.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.c b/src/modules/position_estimator_mc/codegen/rtGetNaN.c
new file mode 100755
index 000000000..552770149
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/rtGetNaN.c
@@ -0,0 +1,96 @@
+/*
+ * rtGetNaN.c
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ * MATLAB for code generation function to initialize non-finite, NaN
+ */
+#include "rtGetNaN.h"
+#define NumBitsPerChar 8U
+
+/* Function: rtGetNaN ==================================================
+ * Abstract:
+ * Initialize rtNaN needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetNaN(void)
+{
+ size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+ real_T nan = 0.0;
+ if (bitsPerReal == 32U) {
+ nan = rtGetNaNF();
+ } else {
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ union {
+ LittleEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0xFFF80000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ nan = tmpVal.fltVal;
+ break;
+ }
+
+ case BigEndian:
+ {
+ union {
+ BigEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
+ tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
+ nan = tmpVal.fltVal;
+ break;
+ }
+ }
+ }
+
+ return nan;
+}
+
+/* Function: rtGetNaNF ==================================================
+ * Abstract:
+ * Initialize rtNaNF needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetNaNF(void)
+{
+ IEEESingle nanF = { { 0 } };
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ nanF.wordL.wordLuint = 0xFFC00000U;
+ break;
+ }
+
+ case BigEndian:
+ {
+ nanF.wordL.wordLuint = 0x7FFFFFFFU;
+ break;
+ }
+ }
+
+ return nanF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetNaN.c) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.h b/src/modules/position_estimator_mc/codegen/rtGetNaN.h
new file mode 100755
index 000000000..5acdd9790
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/rtGetNaN.h
@@ -0,0 +1,21 @@
+/*
+ * rtGetNaN.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+#ifndef __RTGETNAN_H__
+#define __RTGETNAN_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetNaN(void);
+extern real32_T rtGetNaNF(void);
+
+#endif
+/* End of code generation (rtGetNaN.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c b/src/modules/position_estimator_mc/codegen/rt_nonfinite.c
new file mode 100755
index 000000000..de121c4a0
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/rt_nonfinite.c
@@ -0,0 +1,87 @@
+/*
+ * rt_nonfinite.c
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ * MATLAB for code generation function to initialize non-finites,
+ * (Inf, NaN and -Inf).
+ */
+#include "rt_nonfinite.h"
+#include "rtGetNaN.h"
+#include "rtGetInf.h"
+
+real_T rtInf;
+real_T rtMinusInf;
+real_T rtNaN;
+real32_T rtInfF;
+real32_T rtMinusInfF;
+real32_T rtNaNF;
+
+/* Function: rt_InitInfAndNaN ==================================================
+ * Abstract:
+ * Initialize the rtInf, rtMinusInf, and rtNaN needed by the
+ * generated code. NaN is initialized as non-signaling. Assumes IEEE.
+ */
+void rt_InitInfAndNaN(size_t realSize)
+{
+ (void) (realSize);
+ rtNaN = rtGetNaN();
+ rtNaNF = rtGetNaNF();
+ rtInf = rtGetInf();
+ rtInfF = rtGetInfF();
+ rtMinusInf = rtGetMinusInf();
+ rtMinusInfF = rtGetMinusInfF();
+}
+
+/* Function: rtIsInf ==================================================
+ * Abstract:
+ * Test if value is infinite
+ */
+boolean_T rtIsInf(real_T value)
+{
+ return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
+}
+
+/* Function: rtIsInfF =================================================
+ * Abstract:
+ * Test if single-precision value is infinite
+ */
+boolean_T rtIsInfF(real32_T value)
+{
+ return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
+}
+
+/* Function: rtIsNaN ==================================================
+ * Abstract:
+ * Test if value is not a number
+ */
+boolean_T rtIsNaN(real_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+ return _isnan(value)? TRUE:FALSE;
+#else
+ return (value!=value)? 1U:0U;
+#endif
+}
+
+/* Function: rtIsNaNF =================================================
+ * Abstract:
+ * Test if single-precision value is not a number
+ */
+boolean_T rtIsNaNF(real32_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+ return _isnan((real_T)value)? true:false;
+#else
+ return (value!=value)? 1U:0U;
+#endif
+}
+
+
+/* End of code generation (rt_nonfinite.c) */
diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h b/src/modules/position_estimator_mc/codegen/rt_nonfinite.h
new file mode 100755
index 000000000..3bbcfd087
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/rt_nonfinite.h
@@ -0,0 +1,53 @@
+/*
+ * rt_nonfinite.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+#ifndef __RT_NONFINITE_H__
+#define __RT_NONFINITE_H__
+
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+#include <float.h>
+#endif
+#include <stddef.h>
+#include "rtwtypes.h"
+
+extern real_T rtInf;
+extern real_T rtMinusInf;
+extern real_T rtNaN;
+extern real32_T rtInfF;
+extern real32_T rtMinusInfF;
+extern real32_T rtNaNF;
+extern void rt_InitInfAndNaN(size_t realSize);
+extern boolean_T rtIsInf(real_T value);
+extern boolean_T rtIsInfF(real32_T value);
+extern boolean_T rtIsNaN(real_T value);
+extern boolean_T rtIsNaNF(real32_T value);
+
+typedef struct {
+ struct {
+ uint32_T wordH;
+ uint32_T wordL;
+ } words;
+} BigEndianIEEEDouble;
+
+typedef struct {
+ struct {
+ uint32_T wordL;
+ uint32_T wordH;
+ } words;
+} LittleEndianIEEEDouble;
+
+typedef struct {
+ union {
+ real32_T wordLreal;
+ uint32_T wordLuint;
+ } wordL;
+} IEEESingle;
+
+#endif
+/* End of code generation (rt_nonfinite.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rtwtypes.h b/src/modules/position_estimator_mc/codegen/rtwtypes.h
new file mode 100755
index 000000000..8916e8572
--- /dev/null
+++ b/src/modules/position_estimator_mc/codegen/rtwtypes.h
@@ -0,0 +1,159 @@
+/*
+ * rtwtypes.h
+ *
+ * Code generation for function 'kalman_dlqe2'
+ *
+ * C source code generated on: Thu Feb 14 12:52:29 2013
+ *
+ */
+
+#ifndef __RTWTYPES_H__
+#define __RTWTYPES_H__
+#ifndef TRUE
+# define TRUE (1U)
+#endif
+#ifndef FALSE
+# define FALSE (0U)
+#endif
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+
+#include <limits.h>
+
+/*=======================================================================*
+ * Target hardware information
+ * Device type: Generic->MATLAB Host Computer
+ * Number of bits: char: 8 short: 16 int: 32
+ * long: 32 native word size: 32
+ * Byte ordering: LittleEndian
+ * Signed integer division rounds to: Undefined
+ * Shift right on a signed integer as arithmetic shift: off
+ *=======================================================================*/
+
+/*=======================================================================*
+ * Fixed width word size data types: *
+ * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
+ * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
+ * real32_T, real64_T - 32 and 64 bit floating point numbers *
+ *=======================================================================*/
+
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef double real64_T;
+
+/*===========================================================================*
+ * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
+ * ulong_T, char_T and byte_T. *
+ *===========================================================================*/
+
+typedef double real_T;
+typedef double time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef char char_T;
+typedef char_T byte_T;
+
+/*===========================================================================*
+ * Complex number type definitions *
+ *===========================================================================*/
+#define CREAL_T
+ typedef struct {
+ real32_T re;
+ real32_T im;
+ } creal32_T;
+
+ typedef struct {
+ real64_T re;
+ real64_T im;
+ } creal64_T;
+
+ typedef struct {
+ real_T re;
+ real_T im;
+ } creal_T;
+
+ typedef struct {
+ int8_T re;
+ int8_T im;
+ } cint8_T;
+
+ typedef struct {
+ uint8_T re;
+ uint8_T im;
+ } cuint8_T;
+
+ typedef struct {
+ int16_T re;
+ int16_T im;
+ } cint16_T;
+
+ typedef struct {
+ uint16_T re;
+ uint16_T im;
+ } cuint16_T;
+
+ typedef struct {
+ int32_T re;
+ int32_T im;
+ } cint32_T;
+
+ typedef struct {
+ uint32_T re;
+ uint32_T im;
+ } cuint32_T;
+
+
+/*=======================================================================*
+ * Min and Max: *
+ * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
+ * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
+ *=======================================================================*/
+
+#define MAX_int8_T ((int8_T)(127))
+#define MIN_int8_T ((int8_T)(-128))
+#define MAX_uint8_T ((uint8_T)(255))
+#define MIN_uint8_T ((uint8_T)(0))
+#define MAX_int16_T ((int16_T)(32767))
+#define MIN_int16_T ((int16_T)(-32768))
+#define MAX_uint16_T ((uint16_T)(65535))
+#define MIN_uint16_T ((uint16_T)(0))
+#define MAX_int32_T ((int32_T)(2147483647))
+#define MIN_int32_T ((int32_T)(-2147483647-1))
+#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
+#define MIN_uint32_T ((uint32_T)(0))
+
+/* Logical type definitions */
+#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
+# ifndef false
+# define false (0U)
+# endif
+# ifndef true
+# define true (1U)
+# endif
+#endif
+
+/*
+ * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
+ * for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable)
+ * including the null-termination character. Referenced by
+ * rt_logging.c and rt_matrx.c.
+ */
+#define TMW_NAME_LENGTH_MAX 64
+
+#endif
+#endif
+/* End of code generation (rtwtypes.h) */
diff --git a/src/modules/position_estimator_mc/kalman_dlqe1.m b/src/modules/position_estimator_mc/kalman_dlqe1.m
new file mode 100755
index 000000000..ff939d029
--- /dev/null
+++ b/src/modules/position_estimator_mc/kalman_dlqe1.m
@@ -0,0 +1,3 @@
+function [x_aposteriori] = kalman_dlqe1(A,C,K,x_aposteriori_k,z)
+ x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
+end \ No newline at end of file
diff --git a/src/modules/position_estimator_mc/kalman_dlqe2.m b/src/modules/position_estimator_mc/kalman_dlqe2.m
new file mode 100755
index 000000000..2a50164ef
--- /dev/null
+++ b/src/modules/position_estimator_mc/kalman_dlqe2.m
@@ -0,0 +1,9 @@
+function [x_aposteriori] = kalman_dlqe2(dt,k1,k2,k3,x_aposteriori_k,z)
+ st = 1/2*dt^2;
+ A = [1,dt,st;
+ 0,1,dt;
+ 0,0,1];
+ C=[1,0,0];
+ K = [k1;k2;k3];
+ x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
+end \ No newline at end of file
diff --git a/src/modules/position_estimator_mc/kalman_dlqe3.m b/src/modules/position_estimator_mc/kalman_dlqe3.m
new file mode 100755
index 000000000..4c6421b7f
--- /dev/null
+++ b/src/modules/position_estimator_mc/kalman_dlqe3.m
@@ -0,0 +1,17 @@
+function [x_aposteriori] = kalman_dlqe3(dt,k1,k2,k3,x_aposteriori_k,z,posUpdate,addNoise,sigma)
+ st = 1/2*dt^2;
+ A = [1,dt,st;
+ 0,1,dt;
+ 0,0,1];
+ C=[1,0,0];
+ K = [k1;k2;k3];
+ if addNoise==1
+ noise = sigma*randn(1,1);
+ z = z + noise;
+ end
+ if(posUpdate)
+ x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
+ else
+ x_aposteriori=A*x_aposteriori_k;
+ end
+end \ No newline at end of file
diff --git a/src/modules/position_estimator_mc/module.mk b/src/modules/position_estimator_mc/module.mk
new file mode 100644
index 000000000..40b135ea4
--- /dev/null
+++ b/src/modules/position_estimator_mc/module.mk
@@ -0,0 +1,60 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build the position estimator
+#
+
+MODULE_COMMAND = position_estimator_mc
+
+SRCS = position_estimator_mc_main.c \
+ position_estimator_mc_params.c \
+ codegen/positionKalmanFilter1D_initialize.c \
+ codegen/positionKalmanFilter1D_terminate.c \
+ codegen/positionKalmanFilter1D.c \
+ codegen/rt_nonfinite.c \
+ codegen/rtGetInf.c \
+ codegen/rtGetNaN.c \
+ codegen/positionKalmanFilter1D_dT_initialize.c \
+ codegen/positionKalmanFilter1D_dT_terminate.c \
+ codegen/kalman_dlqe1.c \
+ codegen/kalman_dlqe1_initialize.c \
+ codegen/kalman_dlqe1_terminate.c \
+ codegen/kalman_dlqe2.c \
+ codegen/kalman_dlqe2_initialize.c \
+ codegen/kalman_dlqe2_terminate.c \
+ codegen/kalman_dlqe3.c \
+ codegen/kalman_dlqe3_initialize.c \
+ codegen/kalman_dlqe3_terminate.c \
+ codegen/kalman_dlqe3_data.c \
+ codegen/randn.c
diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D.m b/src/modules/position_estimator_mc/positionKalmanFilter1D.m
new file mode 100755
index 000000000..144ff8c7c
--- /dev/null
+++ b/src/modules/position_estimator_mc/positionKalmanFilter1D.m
@@ -0,0 +1,19 @@
+function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D(A,B,C,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
+%prediction
+ x_apriori=A*x_aposteriori_k+B*u;
+ P_apriori=A*P_aposteriori_k*A'+Q;
+ if abs(u)<thresh
+ x_apriori(2)=decay*x_apriori(2);
+ end
+ %update
+ if gps_update==1
+ y=z-C*x_apriori;
+ S=C*P_apriori*C'+R;
+ K=(P_apriori*C')/S;
+ x_aposteriori=x_apriori+K*y;
+ P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
+ else
+ x_aposteriori=x_apriori;
+ P_aposteriori=P_apriori;
+ end
+end
diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m b/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m
new file mode 100755
index 000000000..f94cce1fb
--- /dev/null
+++ b/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m
@@ -0,0 +1,26 @@
+function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D_dT(dT,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
+ %dynamics
+ A = [1 dT -0.5*dT*dT;
+ 0 1 -dT;
+ 0 0 1];
+ B = [0.5*dT*dT; dT; 0];
+ C = [1 0 0];
+ %prediction
+ x_apriori=A*x_aposteriori_k+B*u;
+ P_apriori=A*P_aposteriori_k*A'+Q;
+ if abs(u)<thresh
+ x_apriori(2)=decay*x_apriori(2);
+ end
+ %update
+ if gps_update==1
+ y=z-C*x_apriori;
+ S=C*P_apriori*C'+R;
+ K=(P_apriori*C')/S;
+ x_aposteriori=x_apriori+K*y;
+ P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
+ else
+ x_aposteriori=x_apriori;
+ P_aposteriori=P_apriori;
+ end
+end
+
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c
new file mode 100755
index 000000000..10dee3f22
--- /dev/null
+++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c
@@ -0,0 +1,510 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ * Author: Damian Aregger <daregger@student.ethz.ch>
+ * Tobias Naegeli <naegelit@student.ethz.ch>
+* Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file position_estimator_main.c
+ * Model-identification based position estimator for multirotors
+ */
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <string.h>
+#include <nuttx/config.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <mavlink/mavlink_log.h>
+#include <poll.h>
+#include <systemlib/geo/geo.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "position_estimator_mc_params.h"
+//#include <uORB/topics/debug_key_value.h>
+#include "codegen/kalman_dlqe2.h"
+#include "codegen/kalman_dlqe2_initialize.h"
+#include "codegen/kalman_dlqe3.h"
+#include "codegen/kalman_dlqe3_initialize.h"
+
+static bool thread_should_exit = false; /**< Deamon exit flag */
+static bool thread_running = false; /**< Deamon status flag */
+static int position_estimator_mc_task; /**< Handle of deamon task / thread */
+
+__EXPORT int position_estimator_mc_main(int argc, char *argv[]);
+
+int position_estimator_mc_thread_main(int argc, char *argv[]);
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+ if (reason)
+ warnx("%s\n", reason);
+ warnx("usage: position_estimator_mc {start|stop|status}");
+ exit(1);
+}
+
+/**
+ * The position_estimator_mc_thread only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int position_estimator_mc_main(int argc, char *argv[])
+{
+ if (argc < 1)
+ usage("missing command");
+
+ if (!strcmp(argv[1], "start")) {
+
+ if (thread_running) {
+ printf("position_estimator_mc already running\n");
+ /* this is not an error */
+ exit(0);
+ }
+
+ thread_should_exit = false;
+ position_estimator_mc_task = task_spawn("position_estimator_mc",
+ SCHED_RR,
+ SCHED_PRIORITY_MAX - 5,
+ 4096,
+ position_estimator_mc_thread_main,
+ (argv) ? (const char **)&argv[2] : (const char **)NULL);
+ exit(0);
+ }
+ if (!strcmp(argv[1], "stop")) {
+ thread_should_exit = true;
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "status")) {
+ if (thread_running) {
+ warnx("position_estimator_mc is running");
+ } else {
+ warnx("position_estimator_mc not started");
+ }
+ exit(0);
+ }
+
+ usage("unrecognized command");
+ exit(1);
+}
+
+/****************************************************************************
+ * main
+ ****************************************************************************/
+int position_estimator_mc_thread_main(int argc, char *argv[])
+{
+ /* welcome user */
+ warnx("[position_estimator_mc] started");
+ int mavlink_fd;
+ mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+ mavlink_log_info(mavlink_fd, "[position_estimator_mc] started");
+
+ /* initialize values */
+ float z[3] = {0, 0, 0}; /* output variables from tangent plane mapping */
+ // float rotMatrix[4] = {1.0f, 0.0f, 0.0f, 1.0f};
+ float x_x_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
+ float x_y_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
+ float x_z_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
+ float x_x_aposteriori[3] = {0.0f, 0.0f, 0.0f};
+ float x_y_aposteriori[3] = {1.0f, 0.0f, 0.0f};
+ float x_z_aposteriori[3] = {1.0f, 0.0f, 0.0f};
+
+ // XXX this is terribly wrong and should actual dT instead
+ const float dT_const_50 = 1.0f/50.0f;
+
+ float addNoise = 0.0f;
+ float sigma = 0.0f;
+ //computed from dlqe in matlab
+ const float K_vicon_50Hz[3] = {0.5297f, 0.9873f, 0.9201f};
+ // XXX implement baro filter
+ const float K_baro[3] = {0.0248f, 0.0377f, 0.0287f};
+ float K[3] = {0.0f, 0.0f, 0.0f};
+ int baro_loop_cnt = 0;
+ int baro_loop_end = 70; /* measurement for 1 second */
+ float p0_Pa = 0.0f; /* to determin while start up */
+ float rho0 = 1.293f; /* standard pressure */
+ const float const_earth_gravity = 9.81f;
+
+ float posX = 0.0f;
+ float posY = 0.0f;
+ float posZ = 0.0f;
+
+ double lat_current;
+ double lon_current;
+ float alt_current;
+
+ float gps_origin_altitude = 0.0f;
+
+ /* Initialize filter */
+ kalman_dlqe2_initialize();
+ kalman_dlqe3_initialize();
+
+ /* declare and safely initialize all structs */
+ struct sensor_combined_s sensor;
+ memset(&sensor, 0, sizeof(sensor));
+ struct vehicle_attitude_s att;
+ memset(&att, 0, sizeof(att));
+ struct vehicle_status_s vehicle_status;
+ memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */
+ struct vehicle_vicon_position_s vicon_pos;
+ memset(&vicon_pos, 0, sizeof(vicon_pos));
+ struct actuator_controls_effective_s act_eff;
+ memset(&act_eff, 0, sizeof(act_eff));
+ struct vehicle_gps_position_s gps;
+ memset(&gps, 0, sizeof(gps));
+ struct vehicle_local_position_s local_pos_est;
+ memset(&local_pos_est, 0, sizeof(local_pos_est));
+ struct vehicle_global_position_s global_pos_est;
+ memset(&global_pos_est, 0, sizeof(global_pos_est));
+
+ /* subscribe */
+ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+ int sub_params = orb_subscribe(ORB_ID(parameter_update));
+ int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+ int vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
+ int actuator_eff_sub = orb_subscribe(ORB_ID(actuator_controls_effective_0));
+ int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+ int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+
+ /* advertise */
+ orb_advert_t local_pos_est_pub = 0;
+ orb_advert_t global_pos_est_pub = 0;
+
+ struct position_estimator_mc_params pos1D_params;
+ struct position_estimator_mc_param_handles pos1D_param_handles;
+ /* initialize parameter handles */
+ parameters_init(&pos1D_param_handles);
+
+ bool flag_use_gps = false;
+ bool flag_use_baro = false;
+ bool flag_baro_initialized = false; /* in any case disable baroINITdone */
+ /* FIRST PARAMETER READ at START UP*/
+ struct parameter_update_s update;
+ orb_copy(ORB_ID(parameter_update), sub_params, &update); /* read from param to clear updated flag */
+ /* FIRST PARAMETER UPDATE */
+ parameters_update(&pos1D_param_handles, &pos1D_params);
+ flag_use_baro = pos1D_params.baro;
+ sigma = pos1D_params.sigma;
+ addNoise = pos1D_params.addNoise;
+ /* END FIRST PARAMETER UPDATE */
+
+ /* try to grab a vicon message - if it fails, go for GPS. */
+
+ /* make sure the next orb_check() can't return true unless we get a timely update */
+ orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
+ /* allow 200 ms for vicon to come in */
+ usleep(200000);
+ /* check if we got vicon */
+ bool update_check;
+ orb_check(vicon_pos_sub, &update_check);
+ /* if no update was available, use GPS */
+ flag_use_gps = !update_check;
+
+ if (flag_use_gps) {
+ mavlink_log_info(mavlink_fd, "[pos_est_mc] GPS locked");
+ /* wait until gps signal turns valid, only then can we initialize the projection */
+
+ // XXX magic number
+ float hdop_threshold_m = 4.0f;
+ float vdop_threshold_m = 8.0f;
+
+ /*
+ * If horizontal dilution of precision (hdop / eph)
+ * and vertical diluation of precision (vdop / epv)
+ * are below a certain threshold (e.g. 4 m), AND
+ * home position is not yet set AND the last GPS
+ * GPS measurement is not older than two seconds AND
+ * the system is currently not armed, set home
+ * position to the current position.
+ */
+
+ while (!(gps.fix_type == 3
+ && (gps.eph_m < hdop_threshold_m)
+ && (gps.epv_m < vdop_threshold_m)
+ && (hrt_absolute_time() - gps.timestamp_position < 2000000))) {
+
+ struct pollfd fds1[2] = {
+ { .fd = vehicle_gps_sub, .events = POLLIN },
+ { .fd = sub_params, .events = POLLIN },
+ };
+
+ /* wait for GPS updates, BUT READ VEHICLE STATUS (!)
+ * this choice is critical, since the vehicle status might not
+ * actually change, if this app is started after GPS lock was
+ * aquired.
+ */
+ if (poll(fds1, 2, 5000)) {
+ if (fds1[0].revents & POLLIN){
+ /* Read gps position */
+ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
+ }
+ if (fds1[1].revents & POLLIN){
+ /* Read out parameters to check for an update there, e.g. useGPS variable */
+ /* read from param to clear updated flag */
+ struct parameter_update_s updated;
+ orb_copy(ORB_ID(parameter_update), sub_params, &updated);
+ /* update parameters */
+ parameters_update(&pos1D_param_handles, &pos1D_params);
+ }
+ }
+ static int printcounter = 0;
+ if (printcounter == 100) {
+ printcounter = 0;
+ warnx("[pos_est_mc] wait for GPS fix");
+ }
+ printcounter++;
+ }
+
+ /* get gps value for first initialization */
+ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
+ lat_current = ((double)(gps.lat)) * 1e-7d;
+ lon_current = ((double)(gps.lon)) * 1e-7d;
+ alt_current = gps.alt * 1e-3f;
+ gps_origin_altitude = alt_current;
+ /* initialize coordinates */
+ map_projection_init(lat_current, lon_current);
+ /* publish global position messages only after first GPS message */
+ printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current);
+
+ } else {
+ mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON");
+ /* onboard calculated position estimations */
+ }
+ thread_running = true;
+
+ struct pollfd fds2[3] = {
+ { .fd = vehicle_gps_sub, .events = POLLIN },
+ { .fd = vicon_pos_sub, .events = POLLIN },
+ { .fd = sub_params, .events = POLLIN },
+ };
+
+ bool vicon_updated = false;
+ bool gps_updated = false;
+
+ /**< main_loop */
+ while (!thread_should_exit) {
+ int ret = poll(fds2, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate
+ if (ret < 0) {
+ /* poll error */
+ } else {
+ if (fds2[2].revents & POLLIN){
+ /* new parameter */
+ /* read from param to clear updated flag */
+ struct parameter_update_s updated;
+ orb_copy(ORB_ID(parameter_update), sub_params, &updated);
+ /* update parameters */
+ parameters_update(&pos1D_param_handles, &pos1D_params);
+ flag_use_baro = pos1D_params.baro;
+ sigma = pos1D_params.sigma;
+ addNoise = pos1D_params.addNoise;
+ }
+ vicon_updated = false; /* default is no vicon_updated */
+ if (fds2[1].revents & POLLIN) {
+ /* new vicon position */
+ orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
+ posX = vicon_pos.x;
+ posY = vicon_pos.y;
+ posZ = vicon_pos.z;
+ vicon_updated = true; /* set flag for vicon update */
+ } /* end of poll call for vicon updates */
+ gps_updated = false;
+ if (fds2[0].revents & POLLIN) {
+ /* new GPS value */
+ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
+ /* Project gps lat lon (Geographic coordinate system) to plane*/
+ map_projection_project(((double)(gps.lat)) * 1e-7d, ((double)(gps.lon)) * 1e-7d, &(z[0]), &(z[1]));
+ posX = z[0];
+ posY = z[1];
+ posZ = (float)(gps.alt * 1e-3f);
+ gps_updated = true;
+ }
+
+ /* Main estimator loop */
+ orb_copy(ORB_ID(actuator_controls_effective_0), actuator_eff_sub, &act_eff);
+ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+ orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
+ orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor);
+ // barometric pressure estimation at start up
+ if (!flag_baro_initialized){
+ // mean calculation over several measurements
+ if (baro_loop_cnt<baro_loop_end) {
+ p0_Pa += (sensor.baro_pres_mbar*100);
+ baro_loop_cnt++;
+ } else {
+ p0_Pa /= (float)(baro_loop_cnt);
+ flag_baro_initialized = true;
+ char *baro_m_start = "barometer initialized with p0 = ";
+ char p0_char[15];
+ sprintf(p0_char, "%8.2f", (double)(p0_Pa/100));
+ char *baro_m_end = " mbar";
+ char str[80];
+ strcpy(str,baro_m_start);
+ strcat(str,p0_char);
+ strcat(str,baro_m_end);
+ mavlink_log_info(mavlink_fd, str);
+ }
+ }
+ if (flag_use_gps) {
+ /* initialize map projection with the last estimate (not at full rate) */
+ if (gps.fix_type > 2) {
+ /* x-y-position/velocity estimation in earth frame = gps frame */
+ kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,gps_updated,0.0f,0.0f,x_x_aposteriori);
+ memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
+ kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,gps_updated,0.0f,0.0f,x_y_aposteriori);
+ memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
+ /* z-position/velocity estimation in earth frame = vicon frame */
+ float z_est = 0.0f;
+ if (flag_baro_initialized && flag_use_baro) {
+ z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100))/(rho0*const_earth_gravity);
+ K[0] = K_vicon_50Hz[0];
+ K[1] = K_vicon_50Hz[1];
+ K[2] = K_vicon_50Hz[2];
+ gps_updated = 1.0f; /* always enable the update, cause baro update = 200 Hz */
+ } else {
+ z_est = posZ;
+ K[0] = K_vicon_50Hz[0];
+ K[1] = K_vicon_50Hz[1];
+ K[2] = K_vicon_50Hz[2];
+ }
+
+ kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,gps_updated,0.0f,0.0f,x_z_aposteriori);
+ memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
+ local_pos_est.x = x_x_aposteriori_k[0];
+ local_pos_est.vx = x_x_aposteriori_k[1];
+ local_pos_est.y = x_y_aposteriori_k[0];
+ local_pos_est.vy = x_y_aposteriori_k[1];
+ local_pos_est.z = x_z_aposteriori_k[0];
+ local_pos_est.vz = x_z_aposteriori_k[1];
+ local_pos_est.timestamp = hrt_absolute_time();
+ if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))) {
+ /* publish local position estimate */
+ if (local_pos_est_pub > 0) {
+ orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
+ } else {
+ local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
+ }
+ /* publish on GPS updates */
+ if (gps_updated) {
+
+ double lat, lon;
+ float alt = z_est + gps_origin_altitude;
+
+ map_projection_reproject(local_pos_est.x, local_pos_est.y, &lat, &lon);
+
+ global_pos_est.lat = lat;
+ global_pos_est.lon = lon;
+ global_pos_est.alt = alt;
+
+ if (global_pos_est_pub > 0) {
+ orb_publish(ORB_ID(vehicle_global_position), global_pos_est_pub, &global_pos_est);
+ } else {
+ global_pos_est_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos_est);
+ }
+ }
+ }
+ }
+ } else {
+ /* x-y-position/velocity estimation in earth frame = vicon frame */
+ kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,vicon_updated,addNoise,sigma,x_x_aposteriori);
+ memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
+ kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,vicon_updated,addNoise,sigma,x_y_aposteriori);
+ memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
+ /* z-position/velocity estimation in earth frame = vicon frame */
+ float z_est = 0.0f;
+ float local_sigma = 0.0f;
+ if (flag_baro_initialized && flag_use_baro) {
+ z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100.0f))/(rho0*const_earth_gravity);
+ K[0] = K_vicon_50Hz[0];
+ K[1] = K_vicon_50Hz[1];
+ K[2] = K_vicon_50Hz[2];
+ vicon_updated = 1; /* always enable the update, cause baro update = 200 Hz */
+ local_sigma = 0.0f; /* don't add noise on barometer in any case */
+ } else {
+ z_est = posZ;
+ K[0] = K_vicon_50Hz[0];
+ K[1] = K_vicon_50Hz[1];
+ K[2] = K_vicon_50Hz[2];
+ local_sigma = sigma;
+ }
+ kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,vicon_updated,addNoise,local_sigma,x_z_aposteriori);
+ memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
+ local_pos_est.x = x_x_aposteriori_k[0];
+ local_pos_est.vx = x_x_aposteriori_k[1];
+ local_pos_est.y = x_y_aposteriori_k[0];
+ local_pos_est.vy = x_y_aposteriori_k[1];
+ local_pos_est.z = x_z_aposteriori_k[0];
+ local_pos_est.vz = x_z_aposteriori_k[1];
+ local_pos_est.timestamp = hrt_absolute_time();
+ if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){
+ orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
+ }
+ }
+ } /* end of poll return value check */
+ }
+
+ printf("[pos_est_mc] exiting.\n");
+ mavlink_log_info(mavlink_fd, "[pos_est_mc] exiting");
+ thread_running = false;
+ return 0;
+}
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.c b/src/modules/position_estimator_mc/position_estimator_mc_params.c
new file mode 100755
index 000000000..72e5bc73b
--- /dev/null
+++ b/src/modules/position_estimator_mc/position_estimator_mc_params.c
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: Damian Aregger <daregger@student.ethz.ch>
+ * Tobias Naegeli <naegelit@student.ethz.ch>
+* Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file position_estimator_mc_params.c
+ *
+ * Parameters for position_estimator_mc
+ */
+
+#include "position_estimator_mc_params.h"
+
+/* Kalman Filter covariances */
+/* gps measurement noise standard deviation */
+PARAM_DEFINE_FLOAT(POS_EST_ADDN, 1.0f);
+PARAM_DEFINE_FLOAT(POS_EST_SIGMA, 0.0f);
+PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f);
+PARAM_DEFINE_INT32(POS_EST_BARO, 0.0f);
+
+int parameters_init(struct position_estimator_mc_param_handles *h)
+{
+ h->addNoise = param_find("POS_EST_ADDN");
+ h->sigma = param_find("POS_EST_SIGMA");
+ h->r = param_find("POS_EST_R");
+ h->baro_param_handle = param_find("POS_EST_BARO");
+ return OK;
+}
+
+int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p)
+{
+ param_get(h->addNoise, &(p->addNoise));
+ param_get(h->sigma, &(p->sigma));
+ param_get(h->r, &(p->R));
+ param_get(h->baro_param_handle, &(p->baro));
+ return OK;
+}
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.h b/src/modules/position_estimator_mc/position_estimator_mc_params.h
new file mode 100755
index 000000000..c4c95f7b4
--- /dev/null
+++ b/src/modules/position_estimator_mc/position_estimator_mc_params.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: Damian Aregger <daregger@student.ethz.ch>
+ * Tobias Naegeli <naegelit@student.ethz.ch>
+ * Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file position_estimator_mc_params.h
+ *
+ * Parameters for Position Estimator
+ */
+
+#include <systemlib/param/param.h>
+
+struct position_estimator_mc_params {
+ float addNoise;
+ float sigma;
+ float R;
+ int baro; /* consider barometer */
+};
+
+struct position_estimator_mc_param_handles {
+ param_t addNoise;
+ param_t sigma;
+ param_t r;
+ param_t baro_param_handle;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct position_estimator_mc_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p);
diff --git a/apps/px4io/adc.c b/src/modules/px4iofirmware/adc.c
index 670b8d635..f744698be 100644
--- a/apps/px4io/adc.c
+++ b/src/modules/px4iofirmware/adc.c
@@ -135,6 +135,9 @@ adc_init(void)
return 0;
}
+/*
+ return one measurement, or 0xffff on error
+ */
uint16_t
adc_measure(unsigned channel)
{
@@ -154,9 +157,10 @@ adc_measure(unsigned channel)
while (!(rSR & ADC_SR_EOC)) {
/* never spin forever - this will give a bogus result though */
- if (hrt_elapsed_time(&now) > 1000) {
+ if (hrt_elapsed_time(&now) > 100) {
debug("adc timeout");
- break;
+ perf_end(adc_perf);
+ return 0xffff;
}
}
@@ -165,4 +169,4 @@ adc_measure(unsigned channel)
perf_end(adc_perf);
return result;
-} \ No newline at end of file
+}
diff --git a/apps/px4io/controls.c b/src/modules/px4iofirmware/controls.c
index dc36f6c93..dc36f6c93 100644
--- a/apps/px4io/controls.c
+++ b/src/modules/px4iofirmware/controls.c
diff --git a/apps/px4io/dsm.c b/src/modules/px4iofirmware/dsm.c
index ea35e5513..ea35e5513 100644
--- a/apps/px4io/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
diff --git a/apps/px4io/i2c.c b/src/modules/px4iofirmware/i2c.c
index 4485daa5b..4485daa5b 100644
--- a/apps/px4io/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
diff --git a/apps/px4io/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index f38593d2a..5ada1b220 100644
--- a/apps/px4io/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -174,7 +174,7 @@ mixer_tick(void)
* here.
*/
bool should_arm = (
- /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+ /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
@@ -246,7 +246,7 @@ void
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while fully armed */
- if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+ if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
return;
}
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
new file mode 100644
index 000000000..6379366f4
--- /dev/null
+++ b/src/modules/px4iofirmware/module.mk
@@ -0,0 +1,19 @@
+
+
+SRCS = adc.c \
+ controls.c \
+ dsm.c \
+ i2c.c \
+ px4io.c \
+ registers.c \
+ safety.c \
+ sbus.c \
+ ../systemlib/up_cxxinitialize.c \
+ ../systemlib/hx_stream.c \
+ ../systemlib/perf_counter.c \
+ mixer.cpp \
+ ../systemlib/mixer/mixer.cpp \
+ ../systemlib/mixer/mixer_group.cpp \
+ ../systemlib/mixer/mixer_multirotor.cpp \
+ ../systemlib/mixer/mixer_simple.cpp \
+ \ No newline at end of file
diff --git a/apps/px4io/protocol.h b/src/modules/px4iofirmware/protocol.h
index 8d8b7e941..e575bd841 100644
--- a/apps/px4io/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -115,7 +115,7 @@
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */
#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */
-#define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */
+#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -145,9 +145,9 @@
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
-#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */
+#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
+#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
-#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) /* OK to perform position / vector control (= position lock) */
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
@@ -155,8 +155,6 @@
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */
-#define PX4IO_P_SETUP_IBATT_SCALE 7 /* battery current scaling factor (float) */
-#define PX4IO_P_SETUP_IBATT_BIAS 8 /* battery current bias value */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* autopilot control values, -10000..10000 */
diff --git a/apps/px4io/px4io.c b/src/modules/px4iofirmware/px4io.c
index 9de37e118..bc8dfc116 100644
--- a/apps/px4io/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -64,8 +64,10 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call;
+#ifdef CONFIG_STM32_I2C1
/* store i2c reset count XXX this should be a register, together with other error counters */
volatile uint32_t i2c_loop_resets = 0;
+#endif
/*
* a set of debug buffers to allow us to send debug information from ISRs
@@ -133,7 +135,9 @@ user_start(int argc, char *argv[])
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
+#ifdef CONFIG_ARCH_DMA
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
+#endif
/* print some startup info */
lowsyslog("\nPX4IO: starting\n");
@@ -155,8 +159,10 @@ user_start(int argc, char *argv[])
/* initialise the control inputs */
controls_init();
+#ifdef CONFIG_STM32_I2C1
/* start the i2c handler */
i2c_init();
+#endif
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
diff --git a/apps/px4io/px4io.h b/src/modules/px4iofirmware/px4io.h
index 202e9d9d9..272cdb7bf 100644
--- a/apps/px4io/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -124,10 +124,18 @@ extern struct sys_state_s system_state;
#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
-#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
-#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
-#define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
-#define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+#ifdef GPIO_ACC1_PWR_EN
+ #define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+#endif
+#ifdef GPIO_ACC2_PWR_EN
+ #define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+#endif
+#ifdef GPIO_RELAY1_EN
+ #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+#endif
+#ifdef GPIO_RELAY2_EN
+ #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+#endif
#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT))
#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT))
@@ -148,10 +156,12 @@ extern void mixer_handle_text(const void *buffer, size_t length);
*/
extern void safety_init(void);
+#ifdef CONFIG_STM32_I2C1
/**
* FMU communications
*/
extern void i2c_init(void);
+#endif
/**
* Register space
@@ -183,6 +193,7 @@ extern volatile uint8_t debug_level;
/* send a debug message to the console */
extern void isr_debug(uint8_t level, const char *fmt, ...);
+#ifdef CONFIG_STM32_I2C1
void i2c_dump(void);
void i2c_reset(void);
-
+#endif
diff --git a/apps/px4io/registers.c b/src/modules/px4iofirmware/registers.c
index 6c09def9e..61049c8b6 100644
--- a/apps/px4io/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -138,15 +138,14 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
[PX4IO_P_SETUP_RELAYS] = 0,
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
- [PX4IO_P_SETUP_IBATT_SCALE] = 0,
- [PX4IO_P_SETUP_IBATT_BIAS] = 0,
[PX4IO_P_SETUP_SET_DEBUG] = 0,
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
-#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \
+#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
- PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)
+ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
+ PX4IO_P_SETUP_ARMING_IO_ARM_OK)
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -313,7 +312,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* so that an in-air reset of FMU can not lead to a
* lockup of the IO arming state.
*/
- if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
+ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
}
@@ -364,7 +363,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_PAGE_RC_CONFIG: {
/* do not allow a RC config change while fully armed */
- if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+ if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
break;
}
@@ -508,20 +507,27 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
* Intercept corrected for best results @ 12V.
*/
unsigned counts = adc_measure(ADC_VBATT);
- unsigned mV = (4150 + (counts * 46)) / 10 - 200;
- unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
+ if (counts != 0xffff) {
+ unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+ unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
- r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
+ r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
+ }
}
/* PX4IO_P_STATUS_IBATT */
{
- unsigned counts = adc_measure(ADC_VBATT);
- unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000;
- int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]);
- if (corrected < 0)
- corrected = 0;
- r_page_status[PX4IO_P_STATUS_IBATT] = corrected;
+ /*
+ note that we have no idea what sort of
+ current sensor is attached, so we just
+ return the raw 12 bit ADC value and let the
+ FMU sort it out, with user selectable
+ configuration for their sensor
+ */
+ unsigned counts = adc_measure(ADC_IN5);
+ if (counts != 0xffff) {
+ r_page_status[PX4IO_P_STATUS_IBATT] = counts;
+ }
}
SELECT_PAGE(r_page_status);
diff --git a/apps/px4io/safety.c b/src/modules/px4iofirmware/safety.c
index 5495d5ae1..4dbecc274 100644
--- a/apps/px4io/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,7 +32,8 @@
****************************************************************************/
/**
- * @file Safety button logic.
+ * @file safety.c
+ * Safety button logic.
*/
#include <nuttx/config.h>
@@ -56,11 +57,11 @@ static unsigned counter = 0;
/*
* Define the various LED flash sequences for each system state.
*/
-#define LED_PATTERN_SAFE 0xffff /**< always on */
-#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0xFFFE /**< always on with short break */
-#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */
-#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */
-#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */
+#define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */
+#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */
+#define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */
+#define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */
+#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */
static unsigned blink_counter = 0;
@@ -109,7 +110,8 @@ safety_check_button(void *arg)
* state machine, keep ARM_COUNTER_THRESHOLD the same
* length in all cases of the if/else struct below.
*/
- if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+ (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
@@ -120,8 +122,6 @@ safety_check_button(void *arg)
counter++;
}
- /* Disarm quickly */
-
} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
if (counter < ARM_COUNTER_THRESHOLD) {
@@ -138,21 +138,21 @@ safety_check_button(void *arg)
}
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
- uint16_t pattern = LED_PATTERN_SAFE;
+ uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
- if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
+ if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_IO_FMU_ARMED;
} else {
pattern = LED_PATTERN_IO_ARMED;
}
- } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
+ } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_FMU_ARMED;
+ } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) {
+ pattern = LED_PATTERN_FMU_OK_TO_ARM;
- } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) {
- pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
}
/* Turn the LED on if we have a 1 at the current bit position */
diff --git a/apps/px4io/sbus.c b/src/modules/px4iofirmware/sbus.c
index 073ddeaba..073ddeaba 100644
--- a/apps/px4io/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
diff --git a/apps/sdlog/Makefile b/src/modules/sdlog/module.mk
index 225b14a32..89da2d827 100644
--- a/apps/sdlog/Makefile
+++ b/src/modules/sdlog/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,9 +35,9 @@
# sdlog Application
#
-APPNAME = sdlog
+MODULE_COMMAND = sdlog
# The main thread only buffers to RAM, needs a high priority
-PRIORITY = SCHED_PRIORITY_MAX - 30
-STACKSIZE = 2048
+MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
-include $(APPDIR)/mk/app.mk
+SRCS = sdlog.c \
+ sdlog_ringbuffer.c
diff --git a/apps/sdlog/sdlog.c b/src/modules/sdlog/sdlog.c
index df8745d9f..84a9eb6ac 100644
--- a/apps/sdlog/sdlog.c
+++ b/src/modules/sdlog/sdlog.c
@@ -71,6 +71,7 @@
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/airspeed.h>
#include <systemlib/systemlib.h>
@@ -443,7 +444,8 @@ int sdlog_thread_main(int argc, char *argv[])
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
struct battery_status_s batt;
- struct differential_pressure_s diff_pressure;
+ struct differential_pressure_s diff_pres;
+ struct airspeed_s airspeed;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -461,7 +463,8 @@ int sdlog_thread_main(int argc, char *argv[])
int vicon_pos_sub;
int flow_sub;
int batt_sub;
- int diff_pressure_sub;
+ int diff_pres_sub;
+ int airspeed_sub;
} subs;
/* --- MANAGEMENT - LOGGING COMMAND --- */
@@ -558,11 +561,18 @@ int sdlog_thread_main(int argc, char *argv[])
/* --- DIFFERENTIAL PRESSURE --- */
/* subscribe to ORB for flow measurements */
- subs.diff_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
- fds[fdsc_count].fd = subs.diff_pressure_sub;
+ subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+ fds[fdsc_count].fd = subs.diff_pres_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- AIRSPEED --- */
+ /* subscribe to ORB for airspeed */
+ subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+ fds[fdsc_count].fd = subs.airspeed_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -654,7 +664,8 @@ int sdlog_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
- orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure);
+ orb_copy(ORB_ID(differential_pressure), subs.diff_pres_sub, &buf.diff_pres);
+ orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
/* if skipping is on or logging is disabled, ignore */
@@ -691,9 +702,9 @@ int sdlog_thread_main(int argc, char *argv[])
.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
- .diff_pressure = buf.diff_pressure.differential_pressure_mbar,
- .ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s,
- .true_airspeed = buf.diff_pressure.true_airspeed_m_s
+ .diff_pressure = buf.diff_pres.differential_pressure_pa,
+ .ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
+ .true_airspeed = buf.airspeed.true_airspeed_m_s
};
/* put into buffer for later IO */
diff --git a/apps/sdlog/sdlog_ringbuffer.c b/src/modules/sdlog/sdlog_ringbuffer.c
index d7c8a4759..d7c8a4759 100644
--- a/apps/sdlog/sdlog_ringbuffer.c
+++ b/src/modules/sdlog/sdlog_ringbuffer.c
diff --git a/apps/sdlog/sdlog_ringbuffer.h b/src/modules/sdlog/sdlog_ringbuffer.h
index b65916459..b65916459 100644
--- a/apps/sdlog/sdlog_ringbuffer.h
+++ b/src/modules/sdlog/sdlog_ringbuffer.h
diff --git a/apps/commander/.context b/src/modules/sensors/.context
index e69de29bb..e69de29bb 100644
--- a/apps/commander/.context
+++ b/src/modules/sensors/.context
diff --git a/apps/sensors/Makefile b/src/modules/sensors/module.mk
index 526fb0164..ebbc580e1 100644
--- a/apps/sensors/Makefile
+++ b/src/modules/sensors/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,11 +35,8 @@
# Makefile to build the sensor data collector
#
-APPNAME = sensors
-PRIORITY = SCHED_PRIORITY_MAX-5
-STACKSIZE = 4096
+MODULE_COMMAND = sensors
+MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5"
-CXXSRCS = sensors.cpp
-CSRCS = sensor_params.c
-
-include $(APPDIR)/mk/app.mk
+SRCS = sensors.cpp \
+ sensor_params.c
diff --git a/apps/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 2f93a6be0..04b2cde8b 100644
--- a/apps/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -64,7 +64,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
-PARAM_DEFINE_FLOAT(SENS_VAIR_OFF, 2.5f);
+PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
diff --git a/apps/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 04fa97cfc..560ef6406 100644
--- a/apps/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -77,6 +77,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/airspeed.h>
#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
@@ -98,6 +99,12 @@
#define BAT_VOL_LOWPASS_2 0.01f
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
+/**
+ * HACK - true temperature is much less than indicated temperature in baro,
+ * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
+ */
+#define PCB_TEMP_ESTIMATE_DEG 5.0f
+
#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
@@ -156,6 +163,8 @@ private:
int _mag_sub; /**< raw mag data subscription */
int _rc_sub; /**< raw rc channels data subscription */
int _baro_sub; /**< raw baro data subscription */
+ int _airspeed_sub; /**< airspeed subscription */
+ int _diff_pres_sub; /**< raw differential pressure subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
@@ -165,13 +174,15 @@ private:
orb_advert_t _rc_pub; /**< raw r/c control topic */
orb_advert_t _battery_pub; /**< battery status */
orb_advert_t _airspeed_pub; /**< airspeed */
+ orb_advert_t _diff_pres_pub; /**< differential_pressure */
perf_counter_t _loop_perf; /**< loop performance counter */
struct rc_channels_s _rc; /**< r/c channel data */
struct battery_status_s _battery_status; /**< battery status */
struct baro_report _barometer; /**< barometer data */
- struct differential_pressure_s _differential_pressure;
+ struct differential_pressure_s _diff_pres;
+ struct airspeed_s _airspeed;
struct {
float min[_rc_max_chan_count];
@@ -187,7 +198,7 @@ private:
float mag_scale[3];
float accel_offset[3];
float accel_scale[3];
- float airspeed_offset;
+ int diff_pres_offset_pa;
int rc_type;
@@ -234,7 +245,7 @@ private:
param_t accel_scale[3];
param_t mag_offset[3];
param_t mag_scale[3];
- param_t airspeed_offset;
+ param_t diff_pres_offset_pa;
param_t rc_map_roll;
param_t rc_map_pitch;
@@ -327,6 +338,14 @@ private:
void baro_poll(struct sensor_combined_s &raw);
/**
+ * Poll the differential pressure sensor for updated data.
+ *
+ * @param raw Combined sensor data structure into which
+ * data should be returned.
+ */
+ void diff_pres_poll(struct sensor_combined_s &raw);
+
+ /**
* Check for changes in vehicle status.
*/
void vehicle_status_poll();
@@ -368,7 +387,9 @@ Sensors *g_sensors;
}
Sensors::Sensors() :
+#ifdef CONFIG_HRT_PPM
_ppm_last_valid(0),
+#endif
_fd_adc(-1),
_last_adc(0),
@@ -394,6 +415,7 @@ Sensors::Sensors() :
_rc_pub(-1),
_battery_pub(-1),
_airspeed_pub(-1),
+ _diff_pres_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
@@ -477,8 +499,8 @@ Sensors::Sensors() :
_parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
- /*Airspeed offset */
- _parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF");
+ /* Differential pressure offset */
+ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
@@ -678,7 +700,7 @@ Sensors::parameters_update()
param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2]));
/* Airspeed offset */
- param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset));
+ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
@@ -873,6 +895,32 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
}
void
+Sensors::diff_pres_poll(struct sensor_combined_s &raw)
+{
+ bool updated;
+ orb_check(_diff_pres_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
+
+ raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
+ raw.differential_pressure_counter++;
+
+ _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
+ _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f,
+ raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+
+ /* announce the airspeed if needed, just publish else */
+ if (_airspeed_pub > 0) {
+ orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
+
+ } else {
+ _airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed);
+ }
+ }
+}
+
+void
Sensors::vehicle_status_poll()
{
struct vehicle_status_s vstatus;
@@ -1030,31 +1078,18 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
*/
if (voltage > 0.4f) {
- float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor
-
- float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f,
- _barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa
- // XXX HACK - true temperature is much less than indicated temperature in baro,
- // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
+ float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
- float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
-
- //printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true);
-
- _differential_pressure.timestamp = hrt_absolute_time();
- _differential_pressure.static_pressure_mbar = _barometer.pressure;
- _differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f;
- _differential_pressure.temperature_celcius = _barometer.temperature;
- _differential_pressure.indicated_airspeed_m_s = airspeed_indicated;
- _differential_pressure.true_airspeed_m_s = airspeed_true;
- _differential_pressure.voltage = voltage;
+ _diff_pres.timestamp = hrt_absolute_time();
+ _diff_pres.differential_pressure_pa = diff_pres_pa;
+ _diff_pres.voltage = voltage;
/* announce the airspeed if needed, just publish else */
- if (_airspeed_pub > 0) {
- orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_differential_pressure);
+ if (_diff_pres_pub > 0) {
+ orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres);
} else {
- _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_differential_pressure);
+ _diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres);
}
}
}
@@ -1292,6 +1327,7 @@ Sensors::task_main()
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+ _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -1318,6 +1354,7 @@ Sensors::task_main()
gyro_poll(raw);
mag_poll(raw);
baro_poll(raw);
+ diff_pres_poll(raw);
parameter_update_poll(true /* forced */);
@@ -1366,6 +1403,8 @@ Sensors::task_main()
/* check battery voltage */
adc_poll(raw);
+ diff_pres_poll(raw);
+
/* Inform other processes that new data is available to copy */
if (_publishing)
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
diff --git a/apps/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index 264287b10..15bb833a9 100644
--- a/apps/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -97,7 +97,7 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp
float density = get_air_density(static_pressure, temperature_celsius);
if (density < 0.0001f || !isfinite(density)) {
density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
- printf("[airspeed] Invalid air density, using density at sea level\n");
+// printf("[airspeed] Invalid air density, using density at sea level\n");
}
float pressure_difference = total_pressure - static_pressure;
diff --git a/apps/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h
index def53f0c1..def53f0c1 100644
--- a/apps/systemlib/airspeed.h
+++ b/src/modules/systemlib/airspeed.h
diff --git a/apps/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c
index 321466f87..8aca6a25d 100644
--- a/apps/systemlib/bson/tinybson.c
+++ b/src/modules/systemlib/bson/tinybson.c
@@ -40,7 +40,7 @@
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
-#include <err.h>
+#include <systemlib/err.h>
#include "tinybson.h"
diff --git a/apps/systemlib/bson/tinybson.h b/src/modules/systemlib/bson/tinybson.h
index 666f8191a..666f8191a 100644
--- a/apps/systemlib/bson/tinybson.h
+++ b/src/modules/systemlib/bson/tinybson.h
diff --git a/apps/systemlib/conversions.c b/src/modules/systemlib/conversions.c
index ac94252c5..ac94252c5 100644
--- a/apps/systemlib/conversions.c
+++ b/src/modules/systemlib/conversions.c
diff --git a/apps/systemlib/conversions.h b/src/modules/systemlib/conversions.h
index 5d485b01f..5d485b01f 100644
--- a/apps/systemlib/conversions.h
+++ b/src/modules/systemlib/conversions.h
diff --git a/apps/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index 20b711fa6..20b711fa6 100644
--- a/apps/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
diff --git a/apps/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h
index a97047ea8..a97047ea8 100644
--- a/apps/systemlib/cpuload.h
+++ b/src/modules/systemlib/cpuload.h
diff --git a/apps/systemlib/err.c b/src/modules/systemlib/err.c
index daf17ef8b..daf17ef8b 100644
--- a/apps/systemlib/err.c
+++ b/src/modules/systemlib/err.c
diff --git a/apps/systemlib/err.h b/src/modules/systemlib/err.h
index ca13d6265..ca13d6265 100644
--- a/apps/systemlib/err.h
+++ b/src/modules/systemlib/err.h
diff --git a/apps/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c
index 6746e8ff3..6746e8ff3 100644
--- a/apps/systemlib/geo/geo.c
+++ b/src/modules/systemlib/geo/geo.c
diff --git a/apps/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h
index 0c0b5c533..0c0b5c533 100644
--- a/apps/systemlib/geo/geo.h
+++ b/src/modules/systemlib/geo/geo.h
diff --git a/apps/systemlib/getopt_long.c b/src/modules/systemlib/getopt_long.c
index 27c38635f..27c38635f 100644
--- a/apps/systemlib/getopt_long.c
+++ b/src/modules/systemlib/getopt_long.c
diff --git a/apps/systemlib/getopt_long.h b/src/modules/systemlib/getopt_long.h
index 61e8e76f3..61e8e76f3 100644
--- a/apps/systemlib/getopt_long.h
+++ b/src/modules/systemlib/getopt_long.h
diff --git a/apps/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c
index 8d77f14a8..8d77f14a8 100644
--- a/apps/systemlib/hx_stream.c
+++ b/src/modules/systemlib/hx_stream.c
diff --git a/apps/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h
index 128689953..128689953 100644
--- a/apps/systemlib/hx_stream.h
+++ b/src/modules/systemlib/hx_stream.h
diff --git a/apps/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index df0dfe838..df0dfe838 100644
--- a/apps/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
diff --git a/apps/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 71386cba7..bbfa130a9 100644
--- a/apps/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -418,6 +418,8 @@ public:
enum Geometry {
QUAD_X = 0, /**< quad in X configuration */
QUAD_PLUS, /**< quad in + configuration */
+ QUAD_V, /**< quad in V configuration */
+ QUAD_WIDE, /**< quad in wide configuration */
HEX_X, /**< hex in X configuration */
HEX_PLUS, /**< hex in + configuration */
OCTA_X,
diff --git a/apps/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp
index 1dbc512cd..1dbc512cd 100644
--- a/apps/systemlib/mixer/mixer_group.cpp
+++ b/src/modules/systemlib/mixer/mixer_group.cpp
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 4b9cfc023..8ded0b05c 100644
--- a/apps/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -82,6 +82,18 @@ const MultirotorMixer::Rotor _config_quad_plus[] = {
{ 0.000000, 1.000000, -1.00 },
{ -0.000000, -1.000000, -1.00 },
};
+const MultirotorMixer::Rotor _config_quad_v[] = {
+ { -0.927184, 0.374607, 1.00 },
+ { 0.694658, -0.719340, 1.00 },
+ { 0.927184, 0.374607, -1.00 },
+ { -0.694658, -0.719340, -1.00 },
+};
+const MultirotorMixer::Rotor _config_quad_wide[] = {
+ { -0.927184, 0.374607, 1.00 },
+ { 0.777146, -0.629320, 1.00 },
+ { 0.927184, 0.374607, -1.00 },
+ { -0.777146, -0.629320, -1.00 },
+};
const MultirotorMixer::Rotor _config_hex_x[] = {
{ -1.000000, 0.000000, -1.00 },
{ 1.000000, 0.000000, 1.00 },
@@ -121,6 +133,8 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
&_config_quad_x[0],
&_config_quad_plus[0],
+ &_config_quad_v[0],
+ &_config_quad_wide[0],
&_config_hex_x[0],
&_config_hex_plus[0],
&_config_octa_x[0],
@@ -129,6 +143,8 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME
const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
4, /* quad_x */
4, /* quad_plus */
+ 4, /* quad_v */
+ 4, /* quad_wide */
6, /* hex_x */
6, /* hex_plus */
8, /* octa_x */
@@ -184,6 +200,12 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
} else if (!strcmp(geomname, "4x")) {
geometry = MultirotorMixer::QUAD_X;
+ } else if (!strcmp(geomname, "4v")) {
+ geometry = MultirotorMixer::QUAD_V;
+
+ } else if (!strcmp(geomname, "4w")) {
+ geometry = MultirotorMixer::QUAD_WIDE;
+
} else if (!strcmp(geomname, "6+")) {
geometry = MultirotorMixer::HEX_PLUS;
diff --git a/apps/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp
index 07dc5f37f..07dc5f37f 100644
--- a/apps/systemlib/mixer/mixer_simple.cpp
+++ b/src/modules/systemlib/mixer/mixer_simple.cpp
diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk
new file mode 100644
index 000000000..4d45e1c50
--- /dev/null
+++ b/src/modules/systemlib/mixer/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# mixer library
+#
+LIBNAME = mixerlib
+
+SRCS = mixer.cpp \
+ mixer_group.cpp \
+ mixer_multirotor.cpp \
+ mixer_simple.cpp
diff --git a/apps/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables
index f17ae30ca..683c63040 100755
--- a/apps/systemlib/mixer/multi_tables
+++ b/src/modules/systemlib/mixer/multi_tables
@@ -20,6 +20,20 @@ set quad_plus {
180 CW
}
+set quad_v {
+ 68 CCW
+ -136 CCW
+ -68 CW
+ 136 CW
+}
+
+set quad_wide {
+ 68 CCW
+ -129 CCW
+ -68 CW
+ 129 CW
+}
+
set hex_x {
90 CW
-90 CCW
@@ -60,7 +74,7 @@ set octa_plus {
90 CW
}
-set tables {quad_x quad_plus hex_x hex_plus octa_x octa_plus}
+set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus}
proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
diff --git a/apps/systemlib/Makefile b/src/modules/systemlib/module.mk
index 8240dbe43..fd0289c9a 100644
--- a/apps/systemlib/Makefile
+++ b/src/modules/systemlib/module.mk
@@ -35,7 +35,7 @@
# System utility library
#
-CSRCS = err.c \
+SRCS = err.c \
hx_stream.c \
perf_counter.c \
param/param.c \
@@ -44,17 +44,7 @@ CSRCS = err.c \
cpuload.c \
getopt_long.c \
up_cxxinitialize.c \
- airspeed.c
-
-# ppm_decode.c \
-
-#
-# XXX this really should be a CONFIG_* test
-#
-ifeq ($(TARGET),px4fmu)
-CSRCS += systemlib.c \
pid/pid.c \
- geo/geo.c
-endif
-
-include $(APPDIR)/mk/app.mk
+ geo/geo.c \
+ systemlib.c \
+ airspeed.c
diff --git a/apps/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 8073570d1..69a9bdf9b 100644
--- a/apps/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -46,7 +46,7 @@
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
-#include <err.h>
+#include <systemlib/err.h>
#include <errno.h>
#include <sys/stat.h>
diff --git a/apps/systemlib/param/param.h b/src/modules/systemlib/param/param.h
index 084cd931a..084cd931a 100644
--- a/apps/systemlib/param/param.h
+++ b/src/modules/systemlib/param/param.h
diff --git a/apps/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index 879f4715a..879f4715a 100644
--- a/apps/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
diff --git a/apps/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 5c2cb15b2..5c2cb15b2 100644
--- a/apps/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
diff --git a/apps/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 49315cdc9..49315cdc9 100644
--- a/apps/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
diff --git a/apps/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h
index 64d668867..64d668867 100644
--- a/apps/systemlib/pid/pid.h
+++ b/src/modules/systemlib/pid/pid.h
diff --git a/apps/systemlib/ppm_decode.c b/src/modules/systemlib/ppm_decode.c
index a5d2f738d..a5d2f738d 100644
--- a/apps/systemlib/ppm_decode.c
+++ b/src/modules/systemlib/ppm_decode.c
diff --git a/apps/systemlib/ppm_decode.h b/src/modules/systemlib/ppm_decode.h
index 6c5e15345..6c5e15345 100644
--- a/apps/systemlib/ppm_decode.h
+++ b/src/modules/systemlib/ppm_decode.h
diff --git a/apps/systemlib/scheduling_priorities.h b/src/modules/systemlib/scheduling_priorities.h
index be1dbfcd8..be1dbfcd8 100644
--- a/apps/systemlib/scheduling_priorities.h
+++ b/src/modules/systemlib/scheduling_priorities.h
diff --git a/apps/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index afb7eca29..afb7eca29 100644
--- a/apps/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
diff --git a/apps/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 2c53c648b..2c53c648b 100644
--- a/apps/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
diff --git a/apps/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c
index c78f29570..c78f29570 100644
--- a/apps/systemlib/up_cxxinitialize.c
+++ b/src/modules/systemlib/up_cxxinitialize.c
diff --git a/apps/systemlib/uthash/doc/userguide.txt b/src/modules/systemlib/uthash/doc/userguide.txt
index 3e65a52fc..3e65a52fc 100644
--- a/apps/systemlib/uthash/doc/userguide.txt
+++ b/src/modules/systemlib/uthash/doc/userguide.txt
diff --git a/apps/systemlib/uthash/doc/utarray.txt b/src/modules/systemlib/uthash/doc/utarray.txt
index 37830f124..37830f124 100644
--- a/apps/systemlib/uthash/doc/utarray.txt
+++ b/src/modules/systemlib/uthash/doc/utarray.txt
diff --git a/apps/systemlib/uthash/doc/utlist.txt b/src/modules/systemlib/uthash/doc/utlist.txt
index fbf961c27..fbf961c27 100644
--- a/apps/systemlib/uthash/doc/utlist.txt
+++ b/src/modules/systemlib/uthash/doc/utlist.txt
diff --git a/apps/systemlib/uthash/doc/utstring.txt b/src/modules/systemlib/uthash/doc/utstring.txt
index abfdcd107..abfdcd107 100644
--- a/apps/systemlib/uthash/doc/utstring.txt
+++ b/src/modules/systemlib/uthash/doc/utstring.txt
diff --git a/apps/systemlib/uthash/utarray.h b/src/modules/systemlib/uthash/utarray.h
index 4ffb630bf..4ffb630bf 100644
--- a/apps/systemlib/uthash/utarray.h
+++ b/src/modules/systemlib/uthash/utarray.h
diff --git a/apps/systemlib/uthash/uthash.h b/src/modules/systemlib/uthash/uthash.h
index 9f83fc34f..9f83fc34f 100644
--- a/apps/systemlib/uthash/uthash.h
+++ b/src/modules/systemlib/uthash/uthash.h
diff --git a/apps/systemlib/uthash/utlist.h b/src/modules/systemlib/uthash/utlist.h
index 1578acad2..1578acad2 100644
--- a/apps/systemlib/uthash/utlist.h
+++ b/src/modules/systemlib/uthash/utlist.h
diff --git a/apps/systemlib/uthash/utstring.h b/src/modules/systemlib/uthash/utstring.h
index a181ad778..a181ad778 100644
--- a/apps/systemlib/uthash/utstring.h
+++ b/src/modules/systemlib/uthash/utstring.h
diff --git a/apps/systemcmds/calibration/calibration.h b/src/modules/systemlib/visibility.h
index 028853ec8..2c6adc062 100644
--- a/apps/systemcmds/calibration/calibration.h
+++ b/src/modules/systemlib/visibility.h
@@ -31,30 +31,32 @@
*
****************************************************************************/
-#ifndef _CALIBRATION_H
-#define _CALIBRATION_H
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-/****************************************************************************
- * Definitions
- ****************************************************************************/
-
-/****************************************************************************
- * Public Types
- ****************************************************************************/
+/**
+ * @file visibility.h
+ *
+ * Definitions controlling symbol naming and visibility.
+ *
+ * This file is normally included automatically by the build system.
+ */
-/****************************************************************************
- * Public Variables
- ****************************************************************************/
+#ifndef __SYSTEMLIB_VISIBILITY_H
+#define __SYSTEMLIB_VISIBILITY_H
-/****************************************************************************
- * Public Function Prototypes
- ****************************************************************************/
-
-extern int range_cal(int argc, char *argv[]);
-extern int servo_cal(int argc, char *argv[]);
+#ifdef __EXPORT
+# undef __EXPORT
+#endif
+#define __EXPORT __attribute__ ((visibility ("default")))
+#ifdef __PRIVATE
+# undef __PRIVATE
+#endif
+#define __PRIVATE __attribute__ ((visibility ("hidden")))
+
+#ifdef __cplusplus
+# define __BEGIN_DECLS extern "C" {
+# define __END_DECLS }
+#else
+# define __BEGIN_DECLS
+# define __END_DECLS
#endif
+#endif \ No newline at end of file
diff --git a/src/modules/test/foo.c b/src/modules/test/foo.c
new file mode 100644
index 000000000..ff6af031f
--- /dev/null
+++ b/src/modules/test/foo.c
@@ -0,0 +1,4 @@
+int test_main(void)
+{
+ return 0;
+} \ No newline at end of file
diff --git a/src/modules/test/module.mk b/src/modules/test/module.mk
new file mode 100644
index 000000000..abf80eedf
--- /dev/null
+++ b/src/modules/test/module.mk
@@ -0,0 +1,4 @@
+
+MODULE_NAME = test
+SRCS = foo.c
+
diff --git a/apps/multirotor_att_control/Makefile b/src/modules/uORB/module.mk
index 03cf33e43..5ec31ab01 100755..100644
--- a/apps/multirotor_att_control/Makefile
+++ b/src/modules/uORB/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,8 +35,10 @@
# Makefile to build uORB
#
-APPNAME = multirotor_att_control
-PRIORITY = SCHED_PRIORITY_MAX - 15
-STACKSIZE = 2048
+MODULE_COMMAND = uorb
-include $(APPDIR)/mk/app.mk
+# XXX probably excessive, 2048 should be sufficient
+MODULE_STACKSIZE = 4096
+
+SRCS = uORB.cpp \
+ objects_common.cpp
diff --git a/apps/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 136375140..4197f6fb2 100644
--- a/apps/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -122,6 +122,9 @@ ORB_DEFINE(optical_flow, struct optical_flow_s);
#include "topics/omnidirectional_flow.h"
ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s);
+#include "topics/airspeed.h"
+ORB_DEFINE(airspeed, struct airspeed_s);
+
#include "topics/differential_pressure.h"
ORB_DEFINE(differential_pressure, struct differential_pressure_s);
diff --git a/apps/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index 0b50a29ac..b7c4196c0 100644
--- a/apps/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -69,6 +69,7 @@ ORB_DECLARE(actuator_controls_3);
/** global 'actuator output is live' control. */
struct actuator_armed_s {
bool armed; /**< Set to true if system is armed */
+ bool ready_to_arm; /**< Set to true if system is ready to be armed */
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
};
diff --git a/apps/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h
index 088c4fc8f..088c4fc8f 100644
--- a/apps/uORB/topics/actuator_controls_effective.h
+++ b/src/modules/uORB/topics/actuator_controls_effective.h
diff --git a/apps/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h
index bbe429073..bbe429073 100644
--- a/apps/uORB/topics/actuator_outputs.h
+++ b/src/modules/uORB/topics/actuator_outputs.h
diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h
new file mode 100644
index 000000000..a3da3758f
--- /dev/null
+++ b/src/modules/uORB/topics/airspeed.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file airspeed.h
+ *
+ * Definition of airspeed topic
+ */
+
+#ifndef TOPIC_AIRSPEED_H_
+#define TOPIC_AIRSPEED_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Airspeed
+ */
+struct airspeed_s {
+ uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
+ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */
+ float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(airspeed);
+
+#endif
diff --git a/apps/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h
index c40d0d4e5..c40d0d4e5 100644
--- a/apps/uORB/topics/battery_status.h
+++ b/src/modules/uORB/topics/battery_status.h
diff --git a/apps/uORB/topics/debug_key_value.h b/src/modules/uORB/topics/debug_key_value.h
index a9d1b83fd..a9d1b83fd 100644
--- a/apps/uORB/topics/debug_key_value.h
+++ b/src/modules/uORB/topics/debug_key_value.h
diff --git a/apps/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index d5e4bf37e..8ce85213b 100644
--- a/apps/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -49,16 +49,14 @@
*/
/**
- * Differential pressure and airspeed
+ * Differential pressure.
*/
struct differential_pressure_s {
- uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
- float static_pressure_mbar; /**< Static / environment pressure */
- float differential_pressure_mbar; /**< Differential pressure reading */
- float temperature_celcius; /**< ambient temperature in celcius, -1 if unknown */
- float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */
- float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
- float voltage; /**< Voltage from the airspeed sensor (voltage divider already compensated) */
+ uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
+ uint16_t differential_pressure_pa; /**< Differential pressure reading */
+ uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */
+ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
+
};
/**
diff --git a/apps/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 7e1b82a0f..7e1b82a0f 100644
--- a/apps/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
diff --git a/apps/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index cfee81ea2..cfee81ea2 100644
--- a/apps/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
diff --git a/apps/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 2e895c59c..2e895c59c 100644
--- a/apps/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
diff --git a/apps/uORB/topics/omnidirectional_flow.h b/src/modules/uORB/topics/omnidirectional_flow.h
index 8f4be3b3f..8f4be3b3f 100644
--- a/apps/uORB/topics/omnidirectional_flow.h
+++ b/src/modules/uORB/topics/omnidirectional_flow.h
diff --git a/apps/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h
index c854f0079..c854f0079 100644
--- a/apps/uORB/topics/optical_flow.h
+++ b/src/modules/uORB/topics/optical_flow.h
diff --git a/apps/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h
index 300e895c7..300e895c7 100644
--- a/apps/uORB/topics/parameter_update.h
+++ b/src/modules/uORB/topics/parameter_update.h
diff --git a/apps/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index a0bb25af4..a0bb25af4 100644
--- a/apps/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
diff --git a/apps/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h
index 961ee8b4a..9a76b5182 100644
--- a/apps/uORB/topics/sensor_combined.h
+++ b/src/modules/uORB/topics/sensor_combined.h
@@ -103,6 +103,8 @@ struct sensor_combined_s {
float mcu_temp_celcius; /**< Internal temperature measurement of MCU */
uint32_t baro_counter; /**< Number of raw baro measurements taken */
+ float differential_pressure_pa; /**< Airspeed sensor differential pressure */
+ uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */
};
/**
diff --git a/apps/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h
index c415e832e..c415e832e 100644
--- a/apps/uORB/topics/subsystem_info.h
+++ b/src/modules/uORB/topics/subsystem_info.h
diff --git a/apps/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
index c31c81d0c..c31c81d0c 100755
--- a/apps/uORB/topics/vehicle_attitude.h
+++ b/src/modules/uORB/topics/vehicle_attitude.h
diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index a7cda34a8..a7cda34a8 100644
--- a/apps/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
diff --git a/apps/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index fac571659..fac571659 100644
--- a/apps/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
diff --git a/apps/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index f036c7223..f036c7223 100644
--- a/apps/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
diff --git a/apps/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
index 318abba89..318abba89 100644
--- a/apps/uORB/topics/vehicle_global_position_set_triplet.h
+++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
diff --git a/apps/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
index eec6a8229..eec6a8229 100644
--- a/apps/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
diff --git a/apps/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 5463a460d..0a7fb4e9d 100644
--- a/apps/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -55,38 +55,39 @@
*/
struct vehicle_gps_position_s
{
- uint64_t timestamp_position; /**< Timestamp for position information */
- int32_t lat; /**< Latitude in 1E7 degrees */
- int32_t lon; /**< Longitude in 1E7 degrees */
- int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */
+ uint64_t timestamp_position; /**< Timestamp for position information */
+ int32_t lat; /**< Latitude in 1E7 degrees */
+ int32_t lon; /**< Longitude in 1E7 degrees */
+ int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */
uint64_t timestamp_variance;
float s_variance_m_s; /**< speed accuracy estimate m/s */
- float p_variance_m; /**< position accuracy estimate m */
- uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
+ float p_variance_m; /**< position accuracy estimate m */
+ float c_variance_rad; /**< course accuracy estimate rad */
+ uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
- float eph_m; /**< GPS HDOP horizontal dilution of position in m */
- float epv_m; /**< GPS VDOP horizontal dilution of position in m */
+ float eph_m; /**< GPS HDOP horizontal dilution of position in m */
+ float epv_m; /**< GPS VDOP horizontal dilution of position in m */
- uint64_t timestamp_velocity; /**< Timestamp for velocity informations */
- float vel_m_s; /**< GPS ground speed (m/s) */
- float vel_n_m_s; /**< GPS ground speed in m/s */
- float vel_e_m_s; /**< GPS ground speed in m/s */
- float vel_d_m_s; /**< GPS ground speed in m/s */
- float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad */
- bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */
+ uint64_t timestamp_velocity; /**< Timestamp for velocity informations */
+ float vel_m_s; /**< GPS ground speed (m/s) */
+ float vel_n_m_s; /**< GPS ground speed in m/s */
+ float vel_e_m_s; /**< GPS ground speed in m/s */
+ float vel_d_m_s; /**< GPS ground speed in m/s */
+ float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
+ bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */
uint64_t timestamp_time; /**< Timestamp for time information */
uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */
- uint64_t timestamp_satellites; /**< Timestamp for sattelite information */
+ uint64_t timestamp_satellites; /**< Timestamp for sattelite information */
uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */
uint8_t satellite_prn[20]; /**< Global satellite ID */
uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */
- uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
- uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+ uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+ uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */
- bool satellite_info_available; /**< 0 for no info, 1 for info available */
+ bool satellite_info_available; /**< 0 for no info, 1 for info available */
};
/**
diff --git a/apps/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 76eddeacd..76eddeacd 100644
--- a/apps/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
diff --git a/apps/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
index d24d81e3a..d24d81e3a 100644
--- a/apps/uORB/topics/vehicle_local_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
diff --git a/apps/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h
index 46e62c4b7..46e62c4b7 100644
--- a/apps/uORB/topics/vehicle_rates_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h
diff --git a/apps/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 6d3f8a863..6d3f8a863 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
diff --git a/apps/uORB/topics/vehicle_vicon_position.h b/src/modules/uORB/topics/vehicle_vicon_position.h
index 0822fa89a..0822fa89a 100644
--- a/apps/uORB/topics/vehicle_vicon_position.h
+++ b/src/modules/uORB/topics/vehicle_vicon_position.h
diff --git a/apps/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index 7abbf42ae..7abbf42ae 100644
--- a/apps/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
diff --git a/apps/uORB/uORB.h b/src/modules/uORB/uORB.h
index 82ff46ad2..82ff46ad2 100644
--- a/apps/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
diff --git a/apps/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c
index 45715b791..0569d89f5 100644
--- a/apps/systemcmds/bl_update/bl_update.c
+++ b/src/systemcmds/bl_update/bl_update.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
diff --git a/apps/drivers/boards/px4io/Makefile b/src/systemcmds/bl_update/module.mk
index 85806fe6f..e8eea045e 100644
--- a/apps/drivers/boards/px4io/Makefile
+++ b/src/systemcmds/bl_update/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -32,10 +32,12 @@
############################################################################
#
-# Board-specific startup code for the PX4IO
+# Bootloader updater (flashes the FMU USB bootloader software)
#
-INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
-LIBNAME = brd_px4io
+MODULE_COMMAND = bl_update
+SRCS = bl_update.c
-include $(APPDIR)/mk/app.mk
+MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION = -Os
diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/src/systemcmds/boardinfo/boardinfo.c
index fb8b07ef4..3ff5a066c 100644
--- a/apps/systemcmds/boardinfo/boardinfo.c
+++ b/src/systemcmds/boardinfo/boardinfo.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
diff --git a/src/systemcmds/boardinfo/module.mk b/src/systemcmds/boardinfo/module.mk
new file mode 100644
index 000000000..6851d229b
--- /dev/null
+++ b/src/systemcmds/boardinfo/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Information about FMU and IO boards connected
+#
+
+MODULE_COMMAND = boardinfo
+SRCS = boardinfo.c
+
+MAXOPTIMIZATION = -Os
diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/eeprom/24xxxx_mtd.c
index e34be44e3..e34be44e3 100644
--- a/apps/systemcmds/eeprom/24xxxx_mtd.c
+++ b/src/systemcmds/eeprom/24xxxx_mtd.c
diff --git a/apps/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c
index 49da51358..49da51358 100644
--- a/apps/systemcmds/eeprom/eeprom.c
+++ b/src/systemcmds/eeprom/eeprom.c
diff --git a/src/systemcmds/eeprom/module.mk b/src/systemcmds/eeprom/module.mk
new file mode 100644
index 000000000..07f3945be
--- /dev/null
+++ b/src/systemcmds/eeprom/module.mk
@@ -0,0 +1,39 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# EEPROM file system driver
+#
+
+MODULE_COMMAND = eeprom
+SRCS = 24xxxx_mtd.c eeprom.c
diff --git a/apps/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c
index e1babdc12..4da238039 100644
--- a/apps/systemcmds/i2c/i2c.c
+++ b/src/systemcmds/i2c/i2c.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
diff --git a/apps/systemcmds/i2c/Makefile b/src/systemcmds/i2c/module.mk
index 046e74757..ab2357c7d 100644
--- a/apps/systemcmds/i2c/Makefile
+++ b/src/systemcmds/i2c/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,8 +35,7 @@
# Build the i2c test tool.
#
-APPNAME = i2c
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 4096
+MODULE_COMMAND = i2c
+SRCS = i2c.c
-include $(APPDIR)/mk/app.mk
+MAXOPTIMIZATION = -Os
diff --git a/apps/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.c
index 55c4f0836..55c4f0836 100644
--- a/apps/systemcmds/mixer/mixer.c
+++ b/src/systemcmds/mixer/mixer.c
diff --git a/apps/systemcmds/mixer/Makefile b/src/systemcmds/mixer/module.mk
index b016ddc57..d5a3f9f77 100644
--- a/apps/systemcmds/mixer/Makefile
+++ b/src/systemcmds/mixer/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,8 +35,9 @@
# Build the mixer tool.
#
-APPNAME = mixer
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 4096
+MODULE_COMMAND = mixer
+SRCS = mixer.c
-include $(APPDIR)/mk/app.mk
+MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION = -Os
diff --git a/apps/systemcmds/param/Makefile b/src/systemcmds/param/module.mk
index 603746a20..63f15ad28 100644
--- a/apps/systemcmds/param/Makefile
+++ b/src/systemcmds/param/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,8 +35,10 @@
# Build the parameters tool.
#
-APPNAME = param
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 4096
+MODULE_COMMAND = param
+SRCS = param.c
+
+MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION = -Os
-include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 56f5317e3..60e61d07b 100644
--- a/apps/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
@@ -34,6 +34,7 @@
/**
* @file param.c
+ * @author Lorenz Meier <lm@inf.ethz.ch>
*
* Parameter tool.
*/
@@ -262,7 +263,7 @@ do_set(const char* name, const char* val)
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
- printf("old: %d", i);
+ printf("curr: %d", i);
/* convert string */
char* end;
@@ -276,14 +277,13 @@ do_set(const char* name, const char* val)
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
- printf("float values are not yet supported.");
- // printf("old: %4.4f", (double)f);
-
- // /* convert string */
- // char* end;
- // f = strtof(val,&end);
- // param_set(param, &f);
- // printf(" -> new: %4.4f\n", f);
+ printf("curr: %4.4f", (double)f);
+
+ /* convert string */
+ char* end;
+ f = strtod(val,&end);
+ param_set(param, &f);
+ printf(" -> new: %f\n", f);
}
diff --git a/apps/systemcmds/perf/Makefile b/src/systemcmds/perf/module.mk
index 0134c9948..77952842b 100644
--- a/apps/systemcmds/perf/Makefile
+++ b/src/systemcmds/perf/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,8 +35,7 @@
# perf_counter reporting tool
#
-APPNAME = perf
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 2048
+MODULE_COMMAND = perf
+SRCS = perf.c
-include $(APPDIR)/mk/app.mk
+MAXOPTIMIZATION = -Os
diff --git a/apps/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c
index 64443d019..b69ea597b 100644
--- a/apps/systemcmds/perf/perf.c
+++ b/src/systemcmds/perf/perf.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
diff --git a/src/systemcmds/preflight_check/module.mk b/src/systemcmds/preflight_check/module.mk
new file mode 100644
index 000000000..7c3c88783
--- /dev/null
+++ b/src/systemcmds/preflight_check/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Pre-flight check. Locks down system for a few systems with blinking leds
+# and buzzer if the sensors do not report an OK status.
+#
+
+MODULE_COMMAND = preflight_check
+SRCS = preflight_check.c
+
+MAXOPTIMIZATION = -Os
diff --git a/apps/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index 9ac6f0b5e..42256be61 100644
--- a/apps/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
diff --git a/apps/systemcmds/pwm/Makefile b/src/systemcmds/pwm/module.mk
index 5ab105ed1..4a23bba90 100644
--- a/apps/systemcmds/pwm/Makefile
+++ b/src/systemcmds/pwm/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -35,8 +35,7 @@
# Build the pwm tool.
#
-APPNAME = pwm
-PRIORITY = SCHED_PRIORITY_DEFAULT
-STACKSIZE = 4096
+MODULE_COMMAND = pwm
+SRCS = pwm.c
-include $(APPDIR)/mk/app.mk
+MODULE_STACKSIZE = 4096
diff --git a/apps/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index de7a53199..ff733df52 100644
--- a/apps/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -71,13 +71,14 @@ usage(const char *reason)
warnx("%s", reason);
errx(1,
"usage:\n"
- "pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [arm|disarm] [<channel_value> ...]\n"
+ "pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [-m chanmask ] [arm|disarm] [<channel_value> ...]\n"
" -v Print information about the PWM device\n"
" <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
" <alt_rate> PWM update rate for channels in <alt_channel_mask>\n"
" <channel_group> Channel group that should update at the alternate rate (may be specified more than once)\n"
" arm | disarm Arm or disarm the ouptut\n"
" <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"
+ " <chanmask> Directly supply alt rate channel mask (debug use only)\n"
"\n"
"When -c is specified, any channel groups not listed with -c will update at the default rate.\n"
);
@@ -96,11 +97,12 @@ pwm_main(int argc, char *argv[])
int ret;
char *ep;
unsigned group;
+ int32_t set_mask = -1;
if (argc < 2)
usage(NULL);
- while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) {
+ while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) {
switch (ch) {
case 'c':
group = strtoul(optarg, &ep, 0);
@@ -120,6 +122,12 @@ pwm_main(int argc, char *argv[])
usage("bad alt_rate value");
break;
+ case 'm':
+ set_mask = strtol(optarg, &ep, 0);
+ if (*ep != '\0')
+ usage("bad set_mask value");
+ break;
+
case 'v':
print_info = true;
break;
@@ -143,6 +151,13 @@ pwm_main(int argc, char *argv[])
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
}
+ /* directly supplied channel mask */
+ if (set_mask != -1) {
+ ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
+ if (ret != OK)
+ err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
+ }
+
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
diff --git a/src/systemcmds/reboot/module.mk b/src/systemcmds/reboot/module.mk
new file mode 100644
index 000000000..19f64af54
--- /dev/null
+++ b/src/systemcmds/reboot/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# reboot command.
+#
+
+MODULE_COMMAND = reboot
+SRCS = reboot.c
+
+MAXOPTIMIZATION = -Os
diff --git a/apps/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c
index cc380f94b..cc380f94b 100644
--- a/apps/systemcmds/reboot/reboot.c
+++ b/src/systemcmds/reboot/reboot.c
diff --git a/apps/examples/cdcacm/.context b/src/systemcmds/tests/.context
index e69de29bb..e69de29bb 100644
--- a/apps/examples/cdcacm/.context
+++ b/src/systemcmds/tests/.context
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
new file mode 100644
index 000000000..754d3a0da
--- /dev/null
+++ b/src/systemcmds/tests/module.mk
@@ -0,0 +1,28 @@
+#
+# Assorted tests and the like
+#
+
+MODULE_COMMAND = tests
+MODULE_STACKSIZE = 12000
+MAXOPTIMIZATION = -Os
+
+SRCS = test_adc.c \
+ test_bson.c \
+ test_float.c \
+ test_gpio.c \
+ test_hott_telemetry.c \
+ test_hrt.c \
+ test_int.c \
+ test_jig_voltages.c \
+ test_led.c \
+ test_sensors.c \
+ test_servo.c \
+ test_sleep.c \
+ test_time.c \
+ test_uart_baudchange.c \
+ test_uart_console.c \
+ test_uart_loopback.c \
+ test_uart_send.c \
+ tests_file.c \
+ tests_main.c \
+ tests_param.c
diff --git a/apps/px4/tests/test_adc.c b/src/systemcmds/tests/test_adc.c
index 030ac6e23..030ac6e23 100644
--- a/apps/px4/tests/test_adc.c
+++ b/src/systemcmds/tests/test_adc.c
diff --git a/apps/px4/tests/test_bson.c b/src/systemcmds/tests/test_bson.c
index 6130fe763..6130fe763 100644
--- a/apps/px4/tests/test_bson.c
+++ b/src/systemcmds/tests/test_bson.c
diff --git a/apps/px4/tests/test_float.c b/src/systemcmds/tests/test_float.c
index 4921c9bbb..4921c9bbb 100644
--- a/apps/px4/tests/test_float.c
+++ b/src/systemcmds/tests/test_float.c
diff --git a/apps/px4/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c
index ab536d956..ab536d956 100644
--- a/apps/px4/tests/test_gpio.c
+++ b/src/systemcmds/tests/test_gpio.c
diff --git a/apps/px4/tests/test_hott_telemetry.c b/src/systemcmds/tests/test_hott_telemetry.c
index 74aa0e614..270dc3857 100644
--- a/apps/px4/tests/test_hott_telemetry.c
+++ b/src/systemcmds/tests/test_hott_telemetry.c
@@ -46,6 +46,7 @@
#include <drivers/drv_gpio.h>
#include <nuttx/config.h>
#include <sys/types.h>
+#include <systemlib/err.h>
#include <debug.h>
#include <errno.h>
diff --git a/apps/px4/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c
index f21dd115b..f21dd115b 100644
--- a/apps/px4/tests/test_hrt.c
+++ b/src/systemcmds/tests/test_hrt.c
diff --git a/apps/px4/tests/test_int.c b/src/systemcmds/tests/test_int.c
index c59cee7b7..c59cee7b7 100644
--- a/apps/px4/tests/test_int.c
+++ b/src/systemcmds/tests/test_int.c
diff --git a/apps/px4/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c
index 10c93b264..10c93b264 100644
--- a/apps/px4/tests/test_jig_voltages.c
+++ b/src/systemcmds/tests/test_jig_voltages.c
diff --git a/apps/px4/tests/test_led.c b/src/systemcmds/tests/test_led.c
index 6e3efc668..6e3efc668 100644
--- a/apps/px4/tests/test_led.c
+++ b/src/systemcmds/tests/test_led.c
diff --git a/apps/px4/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index 14503276c..14503276c 100644
--- a/apps/px4/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
diff --git a/apps/px4/tests/test_servo.c b/src/systemcmds/tests/test_servo.c
index f95760ca8..f95760ca8 100644
--- a/apps/px4/tests/test_servo.c
+++ b/src/systemcmds/tests/test_servo.c
diff --git a/apps/px4/tests/test_sleep.c b/src/systemcmds/tests/test_sleep.c
index ae682b542..ae682b542 100644
--- a/apps/px4/tests/test_sleep.c
+++ b/src/systemcmds/tests/test_sleep.c
diff --git a/apps/px4/tests/test_time.c b/src/systemcmds/tests/test_time.c
index 8a164f3fc..8a164f3fc 100644
--- a/apps/px4/tests/test_time.c
+++ b/src/systemcmds/tests/test_time.c
diff --git a/apps/px4/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c
index 609a65c62..609a65c62 100644
--- a/apps/px4/tests/test_uart_baudchange.c
+++ b/src/systemcmds/tests/test_uart_baudchange.c
diff --git a/apps/px4/tests/test_uart_console.c b/src/systemcmds/tests/test_uart_console.c
index f8582b52f..f8582b52f 100644
--- a/apps/px4/tests/test_uart_console.c
+++ b/src/systemcmds/tests/test_uart_console.c
diff --git a/apps/px4/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c
index 3be152004..3be152004 100644
--- a/apps/px4/tests/test_uart_loopback.c
+++ b/src/systemcmds/tests/test_uart_loopback.c
diff --git a/apps/px4/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c
index 7e1e8d307..7e1e8d307 100644
--- a/apps/px4/tests/test_uart_send.c
+++ b/src/systemcmds/tests/test_uart_send.c
diff --git a/apps/px4/tests/tests.h b/src/systemcmds/tests/tests.h
index c02ea6808..c02ea6808 100644
--- a/apps/px4/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
diff --git a/apps/px4/tests/tests_file.c b/src/systemcmds/tests/tests_file.c
index 6f75b9812..6f75b9812 100644
--- a/apps/px4/tests/tests_file.c
+++ b/src/systemcmds/tests/tests_file.c
diff --git a/apps/px4/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 9f8c5c9ea..9f8c5c9ea 100644
--- a/apps/px4/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
diff --git a/apps/px4/tests/tests_param.c b/src/systemcmds/tests/tests_param.c
index 13f17bc43..13f17bc43 100644
--- a/apps/px4/tests/tests_param.c
+++ b/src/systemcmds/tests/tests_param.c
diff --git a/src/systemcmds/top/module.mk b/src/systemcmds/top/module.mk
new file mode 100644
index 000000000..9239aafc3
--- /dev/null
+++ b/src/systemcmds/top/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build top memory / cpu status tool.
+#
+
+MODULE_COMMAND = top
+SRCS = top.c
+
+MODULE_STACKSIZE = 3000
+
+MAXOPTIMIZATION = -Os
+
diff --git a/apps/systemcmds/top/top.c b/src/systemcmds/top/top.c
index 59d2bc8f1..59d2bc8f1 100644
--- a/apps/systemcmds/top/top.c
+++ b/src/systemcmds/top/top.c